//#include "minim.h"
#include <math.h>
#include <iostream>
#include <sstream>
#include "FVLib.h"
#include <time.h>
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------

#define DIM 10000
double XXX[DIM], XXX1[DIM], XXX2[DIM];
double P1[DIM], P2[DIM];
double  RRR[DIM], RRR1[DIM];
double JJJ=0, JJJ0=0;
double nor1, nor2, nor3;
int NNN=4;
double alpha=0, beta=0;

double norma=100000;
double epsilon=0.001;

int leftP[DIM], rightP[DIM];
int sSut[DIM], dSut[DIM], sSutLabels[DIM*3], dSutLabels[DIM*2];
int num_sSuture, num_dSuture;
int left_or_right;
int exec_mode;
double sCoefs[DIM*3], dCoefs[DIM*2]; 
int numSutureVertex;
double leftVals[DIM];//, rightVals[DIM];



// initialize the vector
void initialize(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,
                vector <FVDenseM<double> > &WMat)
{
FVVertex3D *ptr_v;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {
    p[ptr_v->label-1]=ptr_v->coord;   
    }
FVCell3D *ptr_c;   
FVPoint3D<double> FX,FY,FZ,g;
FVDenseM<double> DX(3),DX_bak(3);
double val;
ms.beginCell();
while((ptr_c=ms.nextCell()))
    { 
    //cout<<"cell "<<ptr_c->label<<endl;        
    for(size_t i=0;i<3;i++) 
       {         
          val=ptr_c->vertex[3]->coord.x-ptr_c->vertex[i]->coord.x;DX.setValue(i,0,val);
          val=ptr_c->vertex[3]->coord.y-ptr_c->vertex[i]->coord.y;DX.setValue(i,1,val);
          val=ptr_c->vertex[3]->coord.z-ptr_c->vertex[i]->coord.z;DX.setValue(i,2,val);    
       }
    FX.x=1;FX.y=0;FX.z=0;DX_bak=DX;DX.Gauss(FX);
    FY.x=0;FY.y=1;FY.z=0;DX=DX_bak;DX.Gauss(FY);
    FZ.x=0;FZ.y=0;FZ.z=1;DX=DX_bak;DX.Gauss(FZ);    
    WMat[ptr_c->label-1].resize(3);
    WMat[ptr_c->label-1].setValue(0,0,FX.x); 
    WMat[ptr_c->label-1].setValue(1,0,FX.y);
    WMat[ptr_c->label-1].setValue(2,0,FX.z);
    WMat[ptr_c->label-1].setValue(0,1,FY.x); 
    WMat[ptr_c->label-1].setValue(1,1,FY.y);
    WMat[ptr_c->label-1].setValue(2,1,FY.z); 
    WMat[ptr_c->label-1].setValue(0,2,FZ.x); 
    WMat[ptr_c->label-1].setValue(1,2,FZ.y);
    WMat[ptr_c->label-1].setValue(2,2,FZ.z); 
    }    
}

void show_vector(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p)
{
FVVertex3D *ptr_v;  
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {
    cout<<"label="<<ptr_v->label<<" code="<<ptr_v->code;  
    cout<<"  p.x="<<p[ptr_v->label-1].x<<" p.y="<<p[ptr_v->label-1].y<<" p.z="<<p[ptr_v->label-1].z<<endl;    
    }
}


void dados2XXX(FVMesh3D &ms, FVMesh3D &ms_aux,FVVect<FVPoint3D<double> > &p,Parameter &para)
{   
size_t i;  
static size_t code_fixe,code_chassignac, code_leftSuture, code_rightSuture, code_doubleSutureLeft, code_doubleSutureRight;
static bool no_cond=true;
if(no_cond)
   {
    code_fixe=para.getUnsigned("CodeFixation");
    code_chassignac=para.getUnsigned("CodeChassignac");
    code_leftSuture=para.getUnsigned("CodeLeftSuturation");
    code_rightSuture=para.getUnsigned("CodeRightSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
    no_cond=false;
   }
FVVertex3D *ptr_v, *ptr_vaux;
NNN=0;
ms.beginVertex();
	while((ptr_v=ms.nextVertex()))
    { 
    	i=ptr_v->label-1; 
    	ptr_vaux = ms_aux.getVertex(i);
    	if(ptr_v->code==code_fixe) continue;
     //verify if the vertex belongs to the right or left side of the breast cut section	
        {
     		bool is_on_cut = false;
    		int ptrlabel = ptr_v->label;
        	if(ptr_vaux->code==code_leftSuture || ptr_vaux->code==code_doubleSutureLeft){
        		if(left_or_right==1) is_on_cut=true;
        	}     	
        	if(ptr_vaux->code==code_rightSuture || ptr_vaux->code==code_doubleSutureRight){
        		if(left_or_right==0) is_on_cut=true;
        	}
        	if(is_on_cut==false)
        	{ 
        		if(ptr_v->code==code_chassignac){ XXX[NNN++]=p[i].x;XXX[NNN++]=p[i].z;}
        		else{ XXX[NNN++]=p[i].x;XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z;}
        	}
     	}
     }     
}

void XXX2dados(FVMesh3D &ms, FVMesh3D &ms_aux,FVVect<FVPoint3D<double> > &p,Parameter &para)
{    
size_t i;  
static size_t code_fixe,code_chassignac, code_leftSuture, code_rightSuture, code_doubleSutureLeft, code_doubleSutureRight, code_skin;
static bool no_cond=true;
if(no_cond)
   {
    code_fixe=para.getUnsigned("CodeFixation");
    code_chassignac=para.getUnsigned("CodeChassignac");
    code_leftSuture=para.getUnsigned("CodeLeftSuturation");
    code_rightSuture=para.getUnsigned("CodeRightSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
    code_skin=para.getUnsigned("CodeSkin");
    no_cond=false;
   } 

FVVertex3D *ptr_v, *ptr_vaux;
NNN=0;
ms.beginVertex();
	while((ptr_v=ms.nextVertex()))
    {
    	i=ptr_v->label-1;   
    	ptr_vaux = ms_aux.getVertex(i);       
    	if(ptr_v->code==code_fixe) continue;
    //verify if the vertex belongs to the right or left side of the breast cut section
     	{
     		bool is_on_cut = false;
    		int ptrlabel = ptr_v->label;
        	if(ptr_vaux->code==code_leftSuture || ptr_vaux->code==code_doubleSutureLeft){
        		if(left_or_right==1) is_on_cut=true;
        	}     	
        	if(ptr_vaux->code==code_rightSuture || ptr_vaux->code==code_doubleSutureRight){
        		if(left_or_right==0) is_on_cut=true;
        	}         	
        	if(is_on_cut==false)
        	{ 
        		if(ptr_v->code==code_chassignac){ p[i].x=XXX[NNN++];p[i].z=XXX[NNN++]; /*p[i].y=0.0f;*/}
        		else{ p[i].x=XXX[NNN++];p[i].y=XXX[NNN++];p[i].z=XXX[NNN++];}
        	}
        	else{ 
        		if(ptr_v->code==code_chassignac) ptr_v->code = 112;
        		else if(ptr_v->code==code_skin) ptr_v->code=113;
        			 else ptr_v->code=111;
        	}
     	}
    }
   ms.beginVertex();
	while((ptr_v=ms.nextVertex()))
	{
    	i=ptr_v->label-1;
    	int ptrlabel = ptr_v->label;
    	//obtain new coordinates
    	if((ptr_v->code==111) || (ptr_v->code==112) || (ptr_v->code==113)){
    		for(int it=0; it<num_sSuture; it++){
    			if(sSut[it]==ptrlabel)
        		{
        			p[i].x = sCoefs[it*3]*p[sSutLabels[it*3]-1].x + sCoefs[it*3+1]*p[sSutLabels[it*3+1]-1].x + sCoefs[it*3+2]*p[sSutLabels[it*3+2]-1].x;
        			p[i].y = sCoefs[it*3]*p[sSutLabels[it*3]-1].y + sCoefs[it*3+1]*p[sSutLabels[it*3+1]-1].y + sCoefs[it*3+2]*p[sSutLabels[it*3+2]-1].y;
        			p[i].z = sCoefs[it*3]*p[sSutLabels[it*3]-1].z + sCoefs[it*3+1]*p[sSutLabels[it*3+1]-1].z + sCoefs[it*3+2]*p[sSutLabels[it*3+2]-1].z;
        		}
        	}
        	//reset codes
    		if(ptr_v->code==111){ ptr_v->code = 0;}
    		if(ptr_v->code==113){ ptr_v->code = code_skin;}
    		if(ptr_v->code==112){ ptr_v->code = code_chassignac;}
    	}
    }
}
// -------- evaluation of the functional -----------------
double eval(FVMesh3D &ms, FVMesh3D &ms_aux, FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
            vector <FVDenseM<double> > &WMat,Parameter &para)
{

static bool no_cond=true;
static FVPoint3D<double> g;
static double mu,lambda,muP,lambdaP,skin,coef_chassignac,logParameter,rho;
static size_t  code_energy_surface,code_chassignac;
if(no_cond)
   {
    code_energy_surface=para.getUnsigned("CodeSkin");
    code_chassignac=para.getUnsigned("CodeChassignac");
    g.x=para.getDouble("XGravity");
    g.y=para.getDouble("YGravity");
    g.z=para.getDouble("ZGravity");    
    rho=para.getDouble("Density");
    lambda=para.getDouble("Lambda");
    mu=para.getDouble("Mu");
    lambdaP=para.getDouble("LambdaP");
    muP=para.getDouble("MuP");
    coef_chassignac=para.getDouble("CoefChassignac");
    skin=para.getDouble("SkinThickness");
    logParameter=para.getDouble("LogParameter");
    lambdaP*=skin;muP*=skin;
    no_cond=false;
   }
FVPoint3D<double> FX,FY,FZ;
FVCell3D * ptr_c;
double tr,det,val,sum;
FVDenseM<double> DX(3),DX_bak(3);
sum=0;
ms.beginCell();
while((ptr_c=ms.nextCell()))
    { 
    FX.x=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[0]->label-1].x;
    FX.y=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[1]->label-1].x;
    FX.z=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[2]->label-1].x;
    FY.x=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[0]->label-1].y;
    FY.y=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[1]->label-1].y;
    FY.z=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[2]->label-1].y;
    FZ.x=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[0]->label-1].z;
    FZ.y=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[1]->label-1].z;
    FZ.z=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[2]->label-1].z;            
    WMat[ptr_c->label-1].Mult(FX);
    WMat[ptr_c->label-1].Mult(FY);
    WMat[ptr_c->label-1].Mult(FZ);
    tr=FX.x*FX.x+FX.y*FX.y+FX.z*FX.z+FY.x*FY.x+FY.y*FY.y+FY.z*FY.z+FZ.x*FZ.x+FZ.y*FZ.y+FZ.z*FZ.z;
    det=Det(FX,FY,FZ);if(det<logParameter) det=logParameter;
          
    val=mu*0.5*(tr-3-2*log(det));
    val+=lambda*0.5*(det -1)*(det-1);
    val-=rho*g*(p[ptr_c->vertex[0]->label-1]+p[ptr_c->vertex[1]->label-1]+
                p[ptr_c->vertex[2]->label-1]+p[ptr_c->vertex[3]->label-1])*0.25;

    W[ptr_c->label-1]= val*ptr_c->volume;           
    sum+=W[ptr_c->label-1];
    }
    //printf("\nSum cells:%e\n", sum);
FVFace3D * ptr_f;
FVPoint3D<double> OA,OB,OAp,OBp,aux;
FVPoint2D<double> C1,C2; 
double dOA,dOB,dOAp,dOBp,cosa,sina,cosap,sinap;
ms.beginFace();
while((ptr_f=ms.nextFace()))
    { 
    // we assume that we only treat triangle  
    if(ptr_f->code==code_energy_surface) //energy surface
        {
        OA=ptr_f->vertex[1]->coord-ptr_f->vertex[0]->coord;
        OB=ptr_f->vertex[2]->coord-ptr_f->vertex[0]->coord;
        OAp=p[ptr_f->vertex[1]->label-1]-p[ptr_f->vertex[0]->label-1];
        OBp=p[ptr_f->vertex[2]->label-1]-p[ptr_f->vertex[0]->label-1];
        dOA=Norm(OA);dOB=Norm(OB);
        dOAp=Norm(OAp);dOBp=Norm(OBp);
        cosa=Dot(OA,OB)/dOA/dOB;sina=Norm(CrossProduct(OA,OB))/dOA/dOB;
        cosap=Dot(OAp,OBp)/dOAp/dOBp;sinap=Norm(CrossProduct(OAp,OBp))/dOAp/dOBp;
        C1.x=dOBp/dOB;C1.y=0;
        C2.x=(cosap*dOAp-C1.x*cosa*dOA)/sina/dOA;
        C2.y=sinap*dOAp/sina/dOA;
        tr=C1.x*C1.x+C1.y*C1.y+C2.x*C2.x+C2.y*C2.y;
        det=Det(C1,C2);if(det<logParameter) det=logParameter;
        val=muP*0.5*(tr-2-2*log(det));
        val+=lambdaP*0.5*(det -1)*(det-1);
        WS[ptr_f->label-1]=val*ptr_f->area;
        sum+=WS[ptr_f->label-1]; 
        }
    if(ptr_f->code==code_chassignac) // Chassignac
        {

        OA= p[ptr_f->vertex[0]->label-1]+p[ptr_f->vertex[1]->label-1]+p[ptr_f->vertex[2]->label-1];
        OB= OA/3.-ptr_f->centroid;
        val=(OB.x*OB.x+OB.y*OB.y+OB.z*OB.z)*coef_chassignac;
        //val=(OB.y*OB.y+OB.z*OB.z)*coef_chassignac;
        WS[ptr_f->label-1]=val*ptr_f->area;  
        sum+=WS[ptr_f->label-1]; 
        }
    }
 
    //printf("\nSum cells + faces:%e\n", sum);   
    
    //cout<<"sum="<<sum<<endl;
return(sum);     
}



// -------- evaluation of the functional -----------------
double eval_local(FVMesh3D &ms, FVMesh3D &ms_aux,FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
                  size_t ind, double initial_value,
                  vector <FVDenseM<double> > &WMat,Parameter &para)
{

static bool no_cond=true;
static FVPoint3D<double> g;
static double mu,lambda,muP,lambdaP,skin,coef_chassignac,logParameter,rho;
static size_t  code_energy_surface,code_chassignac,code_fixe, code_leftSuture, code_rightSuture, code_doubleSutureLeft, code_doubleSutureRight, code_skin;
if(no_cond)
   {
    code_energy_surface=para.getUnsigned("CodeSkin");
    code_chassignac=para.getUnsigned("CodeChassignac");
    code_leftSuture=para.getUnsigned("CodeLeftSuturation");
    code_rightSuture=para.getUnsigned("CodeRightSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
    code_fixe=para.getUnsigned("CodeFixation");
    code_skin=para.getUnsigned("CodeSkin");
    g.x=para.getDouble("XGravity");
    g.y=para.getDouble("YGravity");
    g.z=para.getDouble("ZGravity");    
    rho=para.getDouble("Density");
    lambda=para.getDouble("Lambda");
    mu=para.getDouble("Mu");
    lambdaP=para.getDouble("LambdaP");
    muP=para.getDouble("MuP");
    coef_chassignac=para.getDouble("CoefChassignac");
    skin=para.getDouble("SkinThickness");
    logParameter=para.getDouble("LogParameter");
    lambdaP*=skin;muP*=skin;
    no_cond=false;
   }
FVPoint3D<double> FX,FY,FZ;
FVCell3D * ptr_c;
double tr,det,val,sum;
FVDenseM<double> DX(3),DX_bak(3);


// search the vertex
size_t j=0;    
FVVertex3D *ptr_v, *ptr_vv, *ptr_vaux;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    if(ptr_v->code==code_fixe) continue;
    if(ptr_v->code==code_chassignac) 
       {
       	//j+=2;
       	//get dependent values
       	bool is_on_cut = false;
    	int ptrlabel = ptr_v->label;
    	ptr_vaux = ms_aux.getVertex(ptrlabel-1);
        if(ptr_vaux->code==code_leftSuture || ptr_vaux->code==code_doubleSutureLeft){
        	if(left_or_right==1) is_on_cut=true;
        }     	
        if(ptr_vaux->code==code_rightSuture || ptr_vaux->code==code_doubleSutureRight){
        	if(left_or_right==0) is_on_cut=true;
        }        
        if(is_on_cut==false) {j+=2; ptr_v->code=102;}
        else { ptr_v->code=112;}    	
       //j+=2;
       }
    else
       {
       	//j+=3;
       	//get dependent values
       	bool is_on_cut = false;
    	int ptrlabel = ptr_v->label;
    	ptr_vaux = ms_aux.getVertex(ptrlabel-1);
        if(ptr_vaux->code==code_leftSuture || ptr_vaux->code==code_doubleSutureLeft){
        	if(left_or_right==1) is_on_cut=true;
        }     	
        if(ptr_vaux->code==code_rightSuture || ptr_vaux->code==code_doubleSutureRight){
        	if(left_or_right==0) is_on_cut=true;
        }  
        if(is_on_cut==false) {
        	j+=3; 
        	if(ptr_v->code==code_skin) ptr_v->code=103;
        	else ptr_v->code=101;
        }
        else { 
        	if(ptr_v->code==code_skin) ptr_v->code=113;
        	else ptr_v->code=111;
        }
       }    
    if(j>ind) break;   
    }
sum=initial_value;
ptr_v->beginCell();
while((ptr_c=ptr_v->nextCell()))
    { 
    FX.x=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[0]->label-1].x;
    FX.y=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[1]->label-1].x;
    FX.z=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[2]->label-1].x;
    FY.x=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[0]->label-1].y;
    FY.y=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[1]->label-1].y;
    FY.z=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[2]->label-1].y;
    FZ.x=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[0]->label-1].z;
    FZ.y=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[1]->label-1].z;
    FZ.z=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[2]->label-1].z;            
    WMat[ptr_c->label-1].Mult(FX);
    WMat[ptr_c->label-1].Mult(FY);
    WMat[ptr_c->label-1].Mult(FZ); 

    tr=FX.x*FX.x+FX.y*FX.y+FX.z*FX.z+FY.x*FY.x+FY.y*FY.y+FY.z*FY.z+FZ.x*FZ.x+FZ.y*FZ.y+FZ.z*FZ.z;
    det=Det(FX,FY,FZ);if(det<logParameter) det=logParameter;          
    val=mu*0.5*(tr-3-2*log(det));
    val+=lambda*0.5*(det -1)*(det-1);
    val-=rho*g*(p[ptr_c->vertex[0]->label-1]+p[ptr_c->vertex[1]->label-1]+
                p[ptr_c->vertex[2]->label-1]+p[ptr_c->vertex[3]->label-1])*0.25;
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume;           
    }

//-------------------//
//get energy values from the adjacent cut section marked with codes '111' and '112'
//these vertexes depend on the information of the vertexes with codes '101' and '102'
if((ptr_v->code==101) || (ptr_v->code==102) || (ptr_v->code==103)){
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
    for(int ik=0; ik<num_s; ik++)
    {
        if((sSutLabels[ik*3] == ptrlabel) || (sSutLabels[ik*3+1] == ptrlabel) || (sSutLabels[ik*3+2] == ptrlabel)) {
        	ptr_vv=ms.getVertex(sSut[ik]-1);
			ptr_vv->beginCell();
			while((ptr_c=ptr_vv->nextCell()))
    		{ 
    			if(ptr_c->code==999) continue;       
    			FX.x=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[0]->label-1].x;
    			FX.y=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[1]->label-1].x;
    			FX.z=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[2]->label-1].x;
    			FY.x=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[0]->label-1].y;
    			FY.y=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[1]->label-1].y;
    			FY.z=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[2]->label-1].y;
    			FZ.x=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[0]->label-1].z;
    			FZ.y=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[1]->label-1].z;
    			FZ.z=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[2]->label-1].z;            
    			WMat[ptr_c->label-1].Mult(FX);
    			WMat[ptr_c->label-1].Mult(FY);
    			WMat[ptr_c->label-1].Mult(FZ); 
    			tr=FX.x*FX.x+FX.y*FX.y+FX.z*FX.z+FY.x*FY.x+FY.y*FY.y+FY.z*FY.z+FZ.x*FZ.x+FZ.y*FZ.y+FZ.z*FZ.z;
    			det=Det(FX,FY,FZ);if(det<logParameter) det=logParameter;          
    			val=mu*0.5*(tr-3-2*log(det));
    			val+=lambda*0.5*(det -1)*(det-1);
    			val-=rho*g*(p[ptr_c->vertex[0]->label-1]+p[ptr_c->vertex[1]->label-1]+
                	p[ptr_c->vertex[2]->label-1]+p[ptr_c->vertex[3]->label-1])*0.25;
    			sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume;   
    			ptr_c->code = 999;        
    		}
        }
    }
    for(int ik=0; ik<num_s; ik++)
    {
    	ptr_vv=ms.getVertex(sSut[ik]-1);
    	ptr_vv->beginCell();
    	while((ptr_c=ptr_vv->nextCell())){ ptr_c->code=1;}
    }
    
}

//-------------------//

    //cout<<"ok, start the surface energy"<<endl;
FVFace3D * ptr_f;
FVPoint3D<double> OA,OB,OAp,OBp;
FVPoint2D<double> C1,C2; 
double dOA,dOB,dOAp,dOBp,cosa,sina,cosap,sinap;
ptr_v->beginCell();
while((ptr_c=ptr_v->nextCell()))
    {
    ptr_c->beginFace();   
    while((ptr_f=ptr_c->nextFace()))
        { 
        // we assume that we only treat triangle  
        if(ptr_f->code==code_energy_surface) //energy surface
           {
           OA=ptr_f->vertex[1]->coord-ptr_f->vertex[0]->coord;
           OB=ptr_f->vertex[2]->coord-ptr_f->vertex[0]->coord;
           OAp=p[ptr_f->vertex[1]->label-1]-p[ptr_f->vertex[0]->label-1];
           OBp=p[ptr_f->vertex[2]->label-1]-p[ptr_f->vertex[0]->label-1];
           dOA=Norm(OA);dOB=Norm(OB);
           dOAp=Norm(OAp);dOBp=Norm(OBp);
           cosa=Dot(OA,OB)/dOA/dOB;sina=Norm(CrossProduct(OA,OB))/dOA/dOB;
           cosap=Dot(OAp,OBp)/dOAp/dOBp;sinap=Norm(CrossProduct(OAp,OBp))/dOAp/dOBp;
           C1.x=dOBp/dOB;C1.y=0;
           C2.x=(cosap*dOAp-C1.x*cosa*dOA)/sina/dOA;
           C2.y=sinap*dOAp/sina/dOA;
           tr=C1.x*C1.x+C1.y*C1.y+C2.x*C2.x+C2.y*C2.y;
           det=Det(C1,C2);if(det<logParameter) det=logParameter;
           val=muP*0.5*(tr-2-2*log(det));
           val+=lambdaP*0.5*(det -1)*(det-1);
           sum=sum-WS[ptr_f->label-1]+val*ptr_f->area; 
           ptr_f->code=30; // to remeber that the face has been treated
           }
        if(ptr_f->code==code_chassignac) // Chassignac
           {
           OA= p[ptr_f->vertex[0]->label-1]+p[ptr_f->vertex[1]->label-1]+p[ptr_f->vertex[2]->label-1];
           OB= OA/3.-ptr_f->centroid;
           //val=(OB.y*OB.y+OB.z*OB.z)*coef_chassignac;         
           val=(OB.x*OB.x+OB.y*OB.y+OB.z*OB.z)*coef_chassignac; 
           sum=sum-WS[ptr_f->label-1]+val*ptr_f->area; 
           ptr_f->code=20; // to remeber that the face has been treated 
           }   
        }   
    }    
    
//------------------------//

//get energy values from the adjacent cut section marked with codes '111' and '112'
if((ptr_v->code==101) || (ptr_v->code==102) || (ptr_v->code==103)){
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
    for(int ik=0; ik<num_s; ik++)
    {
        if((sSutLabels[ik*3] == ptrlabel) || (sSutLabels[ik*3+1] == ptrlabel) || (sSutLabels[ik*3+2] == ptrlabel)) {
        	ptr_vv=ms.getVertex(sSut[ik]-1); 
			ptr_vv->beginCell();
			while((ptr_c=ptr_vv->nextCell()))
    			{
    			if(ptr_c->code==999) continue;
    			ptr_c->beginFace();   
    			while((ptr_f=ptr_c->nextFace()))
        			{ 
        			// we assume that we only treat triangle  
        			if(ptr_f->code==code_energy_surface) //energy surface
           				{
           				OA=ptr_f->vertex[1]->coord-ptr_f->vertex[0]->coord;
           				OB=ptr_f->vertex[2]->coord-ptr_f->vertex[0]->coord;
           				OAp=p[ptr_f->vertex[1]->label-1]-p[ptr_f->vertex[0]->label-1];
           				OBp=p[ptr_f->vertex[2]->label-1]-p[ptr_f->vertex[0]->label-1];
           				dOA=Norm(OA);dOB=Norm(OB);
           				dOAp=Norm(OAp);dOBp=Norm(OBp);
           				cosa=Dot(OA,OB)/dOA/dOB;sina=Norm(CrossProduct(OA,OB))/dOA/dOB;
           				cosap=Dot(OAp,OBp)/dOAp/dOBp;sinap=Norm(CrossProduct(OAp,OBp))/dOAp/dOBp;
           				C1.x=dOBp/dOB;C1.y=0;
           				C2.x=(cosap*dOAp-C1.x*cosa*dOA)/sina/dOA;
           				C2.y=sinap*dOAp/sina/dOA;
           				tr=C1.x*C1.x+C1.y*C1.y+C2.x*C2.x+C2.y*C2.y;
           				det=Det(C1,C2);if(det<logParameter) det=logParameter;
           				val=muP*0.5*(tr-2-2*log(det));
           				val+=lambdaP*0.5*(det -1)*(det-1);
           				sum=sum-WS[ptr_f->label-1]+val*ptr_f->area; 
           				ptr_f->code=30; // to remeber that the face has been treated
           				}
        			if(ptr_f->code==code_chassignac) // Chassignac
           				{
           				OA= p[ptr_f->vertex[0]->label-1]+p[ptr_f->vertex[1]->label-1]+p[ptr_f->vertex[2]->label-1];
           				OB= OA/3.-ptr_f->centroid;
           				//val=(OB.y*OB.y+OB.z*OB.z)*coef_chassignac;         
           				val=(OB.x*OB.x+OB.y*OB.y+OB.z*OB.z)*coef_chassignac; 
           				sum=sum-WS[ptr_f->label-1]+val*ptr_f->area; 
           				ptr_f->code=20; // to remeber that the face has been treated 
           			}   
        		}
        		ptr_c->code=999;   
    		} 
        }
    }
    for(int ik=0; ik<num_s; ik++)
    {
    	ptr_vv=ms.getVertex(sSut[ik]-1);
    	ptr_vv->beginCell();
    	while((ptr_c=ptr_vv->nextCell())){ ptr_c->code=1;}
    }
}

//------------------------//

// set the good code
ptr_v->beginCell();    
while((ptr_c=ptr_v->nextCell()))
    {
    ptr_c->beginFace();   
    while((ptr_f=ptr_c->nextFace()))  
        {
         if(ptr_f->code==20) ptr_f->code=code_chassignac; // set back the good code           
         if(ptr_f->code==30) ptr_f->code=code_energy_surface; // set back the good code
        }
    }  
    
if((ptr_v->code==101) || (ptr_v->code==102)){
	int num_s = num_sSuture;
	//int ptrlabel = ptr_v->label;
	for(int ik=0; ik<num_s; ik++)
    {
    	ptr_vv=ms.getVertex(sSut[ik]-1);
		ptr_vv->beginCell();    
		while((ptr_c=ptr_vv->nextCell()))
    	{
    		ptr_c->beginFace();   
    		while((ptr_f=ptr_c->nextFace()))  
        	{
        	 if(ptr_f->code==20) ptr_f->code=code_chassignac; // set back the good code           
        	 if(ptr_f->code==30) ptr_f->code=code_energy_surface; // set back the good code
        	}
    	} 
    }
} 

ms.beginVertex();
while((ptr_v=ms.nextVertex()))
{
	if(ptr_v->code==101) ptr_v->code = 0;
	if(ptr_v->code==102) ptr_v->code = code_chassignac;
	if(ptr_v->code==111) ptr_v->code = 0;
	if(ptr_v->code==112) ptr_v->code = code_chassignac;
	//if(ptr_v->code==103) ptr_v->code = code_skin;
	//if(ptr_v->code==113) ptr_v->code = code_skin;
	if(ptr_v->code==103) ptr_v->code = code_skin;
	if(ptr_v->code==113) ptr_v->code = code_skin;
}


return(sum);     
}
//--------------------------------------------------------------

double eval_teste(FVMesh3D &ms, FVMesh3D &ms_aux, FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
            vector <FVDenseM<double> > &WMat,Parameter &para, double skin_val)
{

static bool no_cond=true;
static FVPoint3D<double> g;
static double mu,lambda,muP,lambdaP,skin,coef_chassignac,logParameter,rho;
static size_t  code_energy_surface,code_chassignac;
if(no_cond)
   {
    code_energy_surface=para.getUnsigned("CodeSkin");
    code_chassignac=para.getUnsigned("CodeChassignac");
    g.x=para.getDouble("XGravity");
    g.y=para.getDouble("YGravity");
    g.z=para.getDouble("ZGravity");    
    rho=para.getDouble("Density");
    lambda=para.getDouble("Lambda");
    mu=para.getDouble("Mu");
    lambdaP=para.getDouble("LambdaP");
    muP=para.getDouble("MuP");
    coef_chassignac=para.getDouble("CoefChassignac");
    skin=skin_val;
    logParameter=para.getDouble("LogParameter");
    lambdaP*=skin;muP*=skin;
    no_cond=false;
   }
FVPoint3D<double> FX,FY,FZ;
FVCell3D * ptr_c;
double tr,det,val,sum;
FVDenseM<double> DX(3),DX_bak(3);
sum=0;
double w_sum=0;
ms.beginCell();
while((ptr_c=ms.nextCell()))
    { 
    FX.x=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[0]->label-1].x;
    FX.y=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[1]->label-1].x;
    FX.z=p[ptr_c->vertex[3]->label-1].x-p[ptr_c->vertex[2]->label-1].x;
    FY.x=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[0]->label-1].y;
    FY.y=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[1]->label-1].y;
    FY.z=p[ptr_c->vertex[3]->label-1].y-p[ptr_c->vertex[2]->label-1].y;
    FZ.x=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[0]->label-1].z;
    FZ.y=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[1]->label-1].z;
    FZ.z=p[ptr_c->vertex[3]->label-1].z-p[ptr_c->vertex[2]->label-1].z;            
    WMat[ptr_c->label-1].Mult(FX);
    WMat[ptr_c->label-1].Mult(FY);
    WMat[ptr_c->label-1].Mult(FZ);
    tr=FX.x*FX.x+FX.y*FX.y+FX.z*FX.z+FY.x*FY.x+FY.y*FY.y+FY.z*FY.z+FZ.x*FZ.x+FZ.y*FZ.y+FZ.z*FZ.z;
    det=Det(FX,FY,FZ);if(det<logParameter) det=logParameter;
          
    val=mu*0.5*(tr-3-2*log(det));
    val+=lambda*0.5*(det -1)*(det-1);
    val-=rho*g*(p[ptr_c->vertex[0]->label-1]+p[ptr_c->vertex[1]->label-1]+
                p[ptr_c->vertex[2]->label-1]+p[ptr_c->vertex[3]->label-1])*0.25;

    W[ptr_c->label-1]= val*ptr_c->volume;           
    sum+=W[ptr_c->label-1];
    w_sum+=W[ptr_c->label-1];
    }
    
    printf("\nEnergia::volume:%e\n", w_sum);
FVFace3D * ptr_f;
FVPoint3D<double> OA,OB,OAp,OBp,aux;
FVPoint2D<double> C1,C2; 
double dOA,dOB,dOAp,dOBp,cosa,sina,cosap,sinap;
double ws_sum=0;
ms.beginFace();
while((ptr_f=ms.nextFace()))
    { 
    // we assume that we only treat triangle  
    if(ptr_f->code==code_energy_surface) //energy surface
        {
        OA=ptr_f->vertex[1]->coord-ptr_f->vertex[0]->coord;
        OB=ptr_f->vertex[2]->coord-ptr_f->vertex[0]->coord;
        OAp=p[ptr_f->vertex[1]->label-1]-p[ptr_f->vertex[0]->label-1];
        OBp=p[ptr_f->vertex[2]->label-1]-p[ptr_f->vertex[0]->label-1];
        dOA=Norm(OA);dOB=Norm(OB);
        dOAp=Norm(OAp);dOBp=Norm(OBp);
        cosa=Dot(OA,OB)/dOA/dOB;sina=Norm(CrossProduct(OA,OB))/dOA/dOB;
        cosap=Dot(OAp,OBp)/dOAp/dOBp;sinap=Norm(CrossProduct(OAp,OBp))/dOAp/dOBp;
        C1.x=dOBp/dOB;C1.y=0;
        C2.x=(cosap*dOAp-C1.x*cosa*dOA)/sina/dOA;
        C2.y=sinap*dOAp/sina/dOA;
        tr=C1.x*C1.x+C1.y*C1.y+C2.x*C2.x+C2.y*C2.y;
        det=Det(C1,C2);if(det<logParameter) det=logParameter;
        val=muP*0.5*(tr-2-2*log(det));
        val+=lambdaP*0.5*(det -1)*(det-1);
        WS[ptr_f->label-1]=val*ptr_f->area;
        sum+=WS[ptr_f->label-1]; 
        ws_sum+=WS[ptr_f->label-1]; 
        }
    if(ptr_f->code==code_chassignac) // Chassignac
        {

        OA= p[ptr_f->vertex[0]->label-1]+p[ptr_f->vertex[1]->label-1]+p[ptr_f->vertex[2]->label-1];
        OB= OA/3.-ptr_f->centroid;
        val=(OB.x*OB.x+OB.y*OB.y+OB.z*OB.z)*coef_chassignac;
        //val=(OB.y*OB.y+OB.z*OB.z)*coef_chassignac;
        WS[ptr_f->label-1]=val*ptr_f->area;  
        sum+=WS[ptr_f->label-1]; 
        ws_sum+=WS[ptr_f->label-1]; 
        }
    }
return(sum);     
}


void CalcFun(FVMesh3D &ms, FVMesh3D &ms_aux ,FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
             vector <FVDenseM<double> > &WMat,Parameter &para)
{
  JJJ=0;
  XXX2dados(ms,ms_aux,p,para);
  JJJ= eval(ms,ms_aux,p,W,WS,WMat,para);
return ;
}
//--------------------------------------------------------------


void CalcFun_Local(FVMesh3D &ms, FVMesh3D &ms_aux ,FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
                   size_t ind, double initial_value,
                   vector <FVDenseM<double> > &WMat,Parameter &para)
{
  JJJ=0;
  XXX2dados(ms,ms_aux,p,para);
  JJJ= eval_local(ms,ms_aux,p,W,WS,ind,initial_value,WMat,para);
return ;
}
//---------------------------------------------------------------------------

void CalcGrad(FVMesh3D &ms, FVMesh3D &ms_aux ,FVVect<FVPoint3D<double> > &p ,FVVect<double>  &W, FVVect<double> &WS,
              vector <FVDenseM<double> > &WMat,Parameter &para)
{
static double h;
static bool no_cond=true;
if(no_cond)
    {
    h=para.getDouble("DerivativePrecision");
    no_cond=false;
    }

double YYY=0;
   CalcFun(ms,ms_aux,p,W,WS,WMat,para);
   JJJ0=JJJ;
   for (int i=0; i<NNN; i++) 
       {
       YYY=XXX[i];
       XXX[i]+=h;
       CalcFun_Local(ms,ms_aux,p,W,WS,i,JJJ0,WMat,para);
       //CalcFun(ms,p,W,WS,WMat,para);
       RRR[i]=(JJJ-JJJ0)/h;
       XXX[i]=YYY;
       }
return ;
}

//----------------------------------------------------------------------
void LineSearch_alt1(FVMesh3D &ms , FVMesh3D &ms_aux, FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
                 vector <FVDenseM<double> > &WMat, Parameter &para)
{
	double E = 0.0f, Ep = 1000.0f, Em = 0.0f, eps=1e-6, a_old = 1000.0f, a_new = 0.0f;
	int num_dof = NNN;
	int contador = 0;
	double n_step = 1.0f;
	while((fabs(Ep-Em)/fabs(Ep + Em)) > (eps*eps))
	{
		a_old = a_new;
		for(int i=0; i<num_dof; i++){ XXX[i] = XXX1[i] + a_old*P2[i];}
		CalcFun(ms,ms_aux,p,W,WS,WMat,para); 
		E = JJJ;
		for(int i=0; i<num_dof; i++){ XXX[i] = XXX1[i] + (a_old + eps)*P2[i];}
		CalcFun(ms,ms_aux,p,W,WS,WMat,para); 
		Ep = JJJ;
		for(int i=0; i<num_dof; i++){ XXX[i] = XXX1[i] + (a_old - eps)*P2[i];}
		CalcFun(ms,ms_aux,p,W,WS,WMat,para); 
		Em = JJJ;
		
		a_new = a_old - (n_step) * (((Ep-Em)*eps)/(-2.0*E + Ep + Em))*0.5;
		contador++;
		if((contador+1)%10 == 0) {break;}
	}
	alpha = a_new;
}



void LineSearch1(FVMesh3D &ms, FVMesh3D &ms_aux ,FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, FVVect<double> &WS,
                 vector <FVDenseM<double> > &WMat, Parameter &para)
{
double J1=0, J2=0;
double eps=0.000001;

double a=0;
double b=100;
double c=0;
double e=0;
double g=0.1;

while (fabs(b-a)>eps) {

e=g*(b-a)/2;
c=(a+b)/2;

for (int i=0; i<NNN; i++) {
  XXX[i]=XXX1[i]+(c-e)*P2[i];
}
CalcFun(ms,ms_aux,p,W,WS,WMat,para);
J1=JJJ;

for (int i=0; i<NNN; i++) {
  XXX[i]=XXX1[i]+(c+e)*P2[i];
}
CalcFun(ms,ms_aux,p,W,WS,WMat,para);
J2=JJJ;

if (J1<J2) b=c+e;
if (J1>J2) a=c-e;
if (J1==J2) { a=c-e; b=c+e; }

}//while

alpha=a;
//alpha=b;

return ;
}

//===================================

void LineSearch(FVMesh3D &ms, FVMesh3D &ms_aux ,FVVect<FVPoint3D<double> > &p,FVVect<double>  &W, 
                FVVect<double> &WS,vector <FVDenseM<double> > &WMat,Parameter &para)
{
double J=0;
double eps=0.001;
CalcFun(ms, ms_aux,p,W,WS,WMat,para);
J=JJJ;
int kk=0;
for (int k=1; k<500; k++) {
for (int i=0; i<NNN; i++) {
 XXX[i]=XXX1[i]+k*eps*P2[i];
}

 CalcFun(ms, ms_aux,p,W,WS,WMat,para);
 if (J>JJJ) {
 J=JJJ;
 kk=k;
 }
}

alpha=kk*eps;

return ;
}




double area_tri(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3 ){

	double ax = x3 - x2, bx = x1 - x2;
	double ay = y3 - y2, by = y1 - y2;
	double az = z3 - z2, bz = z1 - z2;
	return (0.5f * sqrt(pow(ay*bz-az*by,2) + pow(ax*bz-az*bx,2) + pow(ax*by-ay*bx,2) ));

}


bool pairCompare(const std::pair<double, int>& firstElem, const std::pair<double, int>& secondElem) {
  return firstElem.first < secondElem.first;

}


void write_mesh(FVMesh3D &ms, FVVect<FVPoint3D<double> > &p, Parameter &para)
{
	
	FVCell3D *ptr_c;
	FVFace3D *ptr_f=NULL,*ptr_lf=NULL;  
	FVEdge3D *ptr_e=NULL,*ptr_le=NULL;
	FVVertex3D *ptr_v=NULL,*ptr_lv=NULL;
	FVPoint3D<double> v, vl, vr, vlx, vrx;
	size_t code_SutureLeft=para.getUnsigned("CodeLeftSuturation");      
	size_t code_SutureRight=para.getUnsigned("CodeRightSuturation");  
	//size_t code_SutureBot=para.getUnsigned("CodeBottomSuturation"); 
	size_t code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
	size_t code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");
	
	size_t nb_vertex=ms.getNbVertex();
	size_t nb_edge=ms.getNbEdge();
	size_t nb_face=ms.getNbFace();
	size_t nb_cell=ms.getNbCell();

	string filename=para.getString("FinalMeshFile");    
	ofstream  out_file;    
	out_file.open(filename.c_str());
	if(!out_file.is_open())
    {
		cout<<" error in file:"<<filename<<"  in "<<__FILE__<< " line "<<__LINE__<<endl; 
		cout<<" can not create the file"<<endl;
        return;
    }
            
	out_file<<"<?xml version=\"1.0\" encoding=\"ISO-8859-1\"?>"<<endl;
	out_file<<"<FVLIB>"<<endl; 
	out_file<<"    <MESH dim=\"3\"    name=\""<<ms.getName()<<"\">"<<endl;
	out_file<<"         <VERTEX nbvertex=\""<<nb_vertex<<"\">"<<endl;
	for(size_t i=0;i<ms.getNbVertex();i++)
    {
		ptr_v=ms.getVertex(i);
		double v_x = p[i].x;
		double v_y = p[i].y;
		double v_z = p[i].z;
		out_file<< setw(FVCHAMPINT)<<ptr_v->label<< setw(FVCHAMPINT)<< ptr_v->code;  
		//out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << p[i].x;
		//out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << p[i].y;
		//out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << p[i].z<<endl;
		out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << v_x;
		out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << v_y;
		out_file<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << v_z<<endl; 
    }
	out_file<<"         </VERTEX>"<<endl; 
	out_file<<"         <EDGE nbedge=\""<<nb_edge<<"\">"<<endl;
	for(size_t i=0;i<ms.getNbEdge();i++)
    {
		ptr_e=ms.getEdge(i);    
		out_file<< setw(FVCHAMPINT)<<ptr_e->label<< setw(FVCHAMPINT)<< ptr_e->code
            << setw(FVCHAMPINT)<< ptr_e->nb_vertex; 
		out_file<<setw(FVCHAMPINT) << ptr_e->firstVertex->label<<setw(FVCHAMPINT) << ptr_e->secondVertex->label<<endl; 
    } 
	out_file<<"         </EDGE>"<<endl; 
	out_file<<"         <FACE nbface=\""<<nb_face<<"\">"<<endl;
	for(size_t i=0;i<ms.getNbFace();i++)
    {
		ptr_f=ms.getFace(i);    
			out_file<< setw(FVCHAMPINT)<<ptr_f->label<< setw(FVCHAMPINT)<< ptr_f->code
            << setw(FVCHAMPINT)<< ptr_f->nb_edge;
		for(size_t j=0;j<ptr_f->nb_edge;j++)        
             out_file<<setw(FVCHAMPINT) << ptr_f->edge[j]->label<<setw(FVCHAMPINT); 
		out_file<<endl; 
    } 
	out_file<<"         </FACE>"<<endl; 
	out_file<<"         <CELL nbcell=\""<<nb_cell<<"\">"<<endl;
	for(size_t i=0;i<nb_cell;i++)
    {
		ptr_c=ms.getCell(i);   
		out_file<< setw(FVCHAMPINT)<<ptr_c->label<< setw(FVCHAMPINT)<< ptr_c->code
            << setw(FVCHAMPINT)<< ptr_c->nb_face;
		for(size_t j=0;j<ptr_c->nb_face;j++)        
             out_file<<setw(FVCHAMPINT) << ptr_c->face[j]->label<<setw(FVCHAMPINT); 
			out_file<<endl; 
    				
    } 
	out_file<<"         </CELL>"<<endl;      
	out_file<<"    </MESH>"<<endl;
	out_file<<"</FVLIB>"<<endl;          
	out_file.close();     

}


//---------------------  MAIN PROGRAM ---------------------------
int main(int argc, char *argv[])
{

// read the datafile
string parameter_filename="param_new.xml";
Parameter para(parameter_filename.c_str());
string mesh_filename, table_filename;
mesh_filename=para.getString("CirurgiaMeshFile");
// read the mesh file
FVMesh3D ms_aux;
ms_aux.read(mesh_filename.c_str());
cout<<" found "<<ms_aux.getNbVertex()<<" vertices, "<<ms_aux.getNbCell()<<" cells, "
               <<ms_aux.getNbFace()<<" faces."<<endl;
// displacement vector
FVVect<FVPoint3D<double> > p(ms_aux.getNbVertex()),F(ms_aux.getNbVertex()), u(ms_aux.getNbVertex());
FVVect<double> W(ms_aux.getNbCell());
FVVect<double> WS(ms_aux.getNbFace());
vector <FVDenseM<double> > WMat(ms_aux.getNbCell());

mesh_filename=para.getString("SuturedMeshFile");
//mesh_filename_aux=para.getString("CirurgiaMeshFile");
// read the mesh file
FVMesh3D ms;
ms.read(mesh_filename.c_str());fflush(NULL);
//ms_aux.read(mesh_filename_aux.c_str());fflush(NULL);
cout<<" found "<<ms.getNbVertex()<<" vertices, "<<ms.getNbCell()<<" cells, "
               <<ms.getNbFace()<<" faces."<<endl;
// displacement vector
FVVect<FVPoint3D<double> > p2(ms.getNbVertex()),F2(ms.getNbVertex()), u2(ms.getNbVertex());
FVVect<double> W2(ms.getNbCell());
FVVect<double> WS2(ms.getNbFace());
vector <FVDenseM<double> > WMat2(ms.getNbCell());



exec_mode = 1; //cirurgia

	//create table
	double search_radius = para.getDouble("search_radius"); 
	search_radius = (exp(search_radius*1.4)-1.0);
	size_t code_SutureLeft=para.getUnsigned("CodeLeftSuturation");      
	size_t code_SutureRight=para.getUnsigned("CodeRightSuturation"); 
	size_t code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
	size_t code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
	size_t code_matchLeft=para.getUnsigned("CodeTableMatchLeft");
	size_t code_matchRight=para.getUnsigned("CodeTableMatchRight"); 
	static size_t code_fixe,code_chassignac,code_leftSuture,code_rightSuture,code_bottomSuture,code_energy_surface;
	//static size_t code_doubleSutureLeft,code_doubleSutureRight;
	
    code_fixe=para.getUnsigned("CodeFixation");
    code_energy_surface=para.getUnsigned("CodeSkin");
    code_chassignac=para.getUnsigned("CodeChassignac");
    code_leftSuture=para.getUnsigned("CodeLeftSuturation");
    code_rightSuture=para.getUnsigned("CodeRightSuturation");
    code_bottomSuture=para.getUnsigned("CodeBottomSuturation");
    //code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    //code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 

	//evaluate number of vertexes of the left face
	//evaluate number of vertexes of the right face
	//compare number and rotate vertexes of the face with less vertexes
	//or
	//rotate vertexes of the right side, to the ones on the left side
	// rotate(alpha + beta), alpha - angle of left side, beta - angle of right side
	
	double num_vertex = ms_aux.getNbVertex();
	FVVertex3D *ptr_v=NULL, *ptr_aux1=NULL, *ptr_aux2=NULL, *ptr_aux3=NULL, *ptr_auxlit=NULL;
	FVVertex3D *ptr_val=NULL, *ptr_val1=NULL, *ptr_val2=NULL, *ptr_val3=NULL;
	int num_l=0, num_r=0, num_dl=0, num_dr=0;
	double alphaTransf = para.getDouble("TransformLeft"), betaTransf = para.getDouble("TransformRight");
	double r_m = para.getDouble("R");
	//double Ax = 0.0f;
	double Az = para.getDouble("S");
	double HH = para.getDouble("H");
	alphaTransf = alphaTransf * M_PI /180.0f;
	betaTransf = betaTransf * M_PI /180.0f;
	//points of the mesh
	FVPoint3D<double> pB, pA, pD, pE, centerD, centerE;
	double min_y_A = 0.0, min_z_D = 1e10, min_z_E = 1e10;
	pB.x = 0.0; pB.y = 0.0; pB.z = Az;
	pA.x = 0.0; pA.z = Az;
	int arc_cL=0, arc_cR=0;
	int pA_label;
	for(int i=0; i<num_vertex; i++)
	{
		ptr_v = ms_aux.getVertex(i);
		double auxX = ptr_v->coord.x, auxY = ptr_v->coord.y, auxZ = ptr_v->coord.z;
		if((auxX > -1e-8) && (auxX < 1e-8) && (auxZ > (Az-1e-8)) && (auxZ < (Az+1e-8))){
			if(auxY<min_y_A){ min_y_A = auxY; pA.y = auxY; pA_label=ptr_v->label;}
		}
		if((auxY > -1e-8) && (auxY < 1e-8)){
			if((auxZ<min_z_D) && (auxX>0.0)){ min_z_D = auxZ; pD.x = auxX; pD.y = auxY; pD.z = auxZ;}
			else if((auxZ<min_z_E) && (auxX<0.0)){ min_z_E = auxZ; pE.x = auxX; pE.y = auxY; pE.z = auxZ;}
		}
		//count the number of vertexes according to its code
		if(ptr_v->code==code_SutureLeft) {num_l++; /*printf("\nssut label -> %d\n", ptr_v->label);*/}
		if(ptr_v->code==code_doubleSutureLeft) {num_dl++; /*printf("\ndsut label -> %d\n", ptr_v->label);*/}
		if(ptr_v->code==code_SutureRight) {
			num_r++;
			//printf("\n ssut dir label -> %d\n", ptr_v->label);
		}
		if(ptr_v->code==code_doubleSutureRight) {
			num_dr++;
			//printf("\n dsut dir label -> %d\n", ptr_v->label);
		}
		if(ptr_v->code==code_matchLeft){
			arc_cL++;
			printf("\n matchleft: %d\n", (int)ptr_v->label);
		}
		if(ptr_v->code==code_matchRight){
			arc_cR++;
			printf("\n matchright: %d\n", (int)ptr_v->label);
		}
	}
	//verify if the transformation is made from the left or from the right
	if(alphaTransf <= betaTransf) left_or_right = 0; //rotate right to left
	else left_or_right = 1; //rotate left to right
	
	//if(left_or_right==0) num_sSuture = num_r + num_dr;
	//else num_sSuture = num_l + num_dl;
	if(left_or_right==0) num_sSuture = num_r + num_dr + arc_cR;
	else num_sSuture = num_l + num_dl + arc_cL;

	//build match tables
	int *table_left = new int[arc_cL];
	int *table_right = new int[arc_cR];
	double *angle_left = new double[arc_cL];
	double *angle_right = new double[arc_cR];
	int *aux_table_left = new int[arc_cL];
	int *aux_table_right = new int[arc_cR];
	double *aux_angle_left = new double[arc_cL];
	double *aux_angle_right = new double[arc_cR];
	double ab_dist = sqrt(pow(pA.x-pB.x,2)+pow(pA.y-pB.y,2)+pow(pA.z-pB.z,2));
	int auxIl=0, auxIr=0;
	for(int i=0; i<num_vertex; i++)
	{
		ptr_v = ms_aux.getVertex(i);
		ptr_val = ms.getVertex(i);
		double auxX = ptr_val->coord.x, auxY = ptr_val->coord.y, auxZ = ptr_val->coord.z;
		double auxA;
		if(ptr_v->code==code_matchLeft){
			auxA = acos( ((pA.x-pB.x)*(auxX-pB.x) + (pA.y-pB.y)*(auxY-pB.y) + (pA.z-pB.z)*(auxZ-pB.z)) / (ab_dist*sqrt(pow(auxX-pB.x,2)+pow(auxY-pB.y,2)+pow(auxZ-pB.z,2))) );
			aux_table_left[auxIl] = ptr_v->label;
			aux_angle_left[auxIl++] = auxA;
		}
		if(ptr_v->code==code_matchRight){
			auxA = acos( ((pA.x-pB.x)*(auxX-pB.x) + (pA.y-pB.y)*(auxY-pB.y) + (pA.z-pB.z)*(auxZ-pB.z)) / (ab_dist*sqrt(pow(auxX-pB.x,2)+pow(auxY-pB.y,2)+pow(auxZ-pB.z,2))) );
			aux_table_right[auxIr] = ptr_v->label;
			aux_angle_right[auxIr++] = auxA;
		}
	}
	double mx_angle;
	int which=-1, which_ind=-1;
	//table left
	for(int i=0; i<arc_cL; i++){
		mx_angle = M_PI*2.0;
		for(int j=0; j<arc_cL; j++){
			if(aux_angle_left[j]<mx_angle){
				mx_angle = aux_angle_left[j];
				which = aux_table_left[j];
				which_ind=j;
			}
		}
		aux_angle_left[which_ind] = M_PI*2.0;
		table_left[i] = which;
		angle_left[i] = mx_angle;
		ptr_v = ms_aux.getVertex(which-1);
		ptr_val = ms.getVertex(which-1);
		ptr_v->code = code_SutureLeft;
		double val_dist = sqrt(pow(pB.x-ptr_val->coord.x,2)+pow(pB.y-ptr_val->coord.y,2)+pow(pB.z-ptr_val->coord.z,2));
		if((mx_angle>(M_PI/2.0-1e-8)) && (mx_angle<(M_PI/2.0+1e-8))) ptr_v->code = code_doubleSutureLeft;
	}
	//table right
	//printf("\nright table\n");
	for(int i=0; i<arc_cR; i++){
		mx_angle = M_PI*2.0;
		for(int j=0; j<arc_cR; j++){
			if(aux_angle_right[j]<mx_angle){
				mx_angle = aux_angle_right[j];
				which = aux_table_right[j];
				which_ind=j;
			}
		}
		aux_angle_right[which_ind] = M_PI*2.0;
		table_right[i] = which;
		angle_right[i] = mx_angle;
		ptr_v = ms_aux.getVertex(which-1);
		ptr_val = ms.getVertex(which-1);
		ptr_v->code = code_SutureRight;
		double val_dist = sqrt(pow(pB.x-ptr_val->coord.x,2)+pow(pB.y-ptr_val->coord.y,2)+pow(pB.z-ptr_val->coord.z,2));
		if((mx_angle>(M_PI/2.0-1e-8)) && (mx_angle<(M_PI/2.0+1e-8))) ptr_v->code = code_doubleSutureRight;
	}
	//printf("\n");
	delete [] aux_table_left;
	delete [] aux_table_right;
	delete [] aux_angle_left;
	delete [] aux_angle_right;
	
	//compute length of the arcs
	ptr_aux1 = ms_aux.getVertex(table_left[0]-1);
	ptr_aux2 = ms_aux.getVertex(table_right[0]-1);
	ptr_val1 = ms.getVertex(table_left[0]-1);
	ptr_val2 = ms.getVertex(table_right[0]-1);
	double arc_length_left=sqrt(pow(pA.x-ptr_val1->coord.x,2) + pow(pA.y-ptr_val1->coord.y,2) + pow(pA.z-ptr_val1->coord.z,2));
	double arc_length_right=sqrt(pow(pA.x-ptr_val2->coord.x,2) + pow(pA.y-ptr_val2->coord.y,2) + pow(pA.z-ptr_val2->coord.z,2));
	
	for(int i=1; i<arc_cL; i++){
		ptr_aux1 = ms_aux.getVertex(table_left[i-1]-1);
		ptr_aux2 = ms_aux.getVertex(table_left[i]-1);
		ptr_val1 = ms.getVertex(table_left[i-1]-1);
		ptr_val2 = ms.getVertex(table_left[i]-1);
		arc_length_left += sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
	}
	for(int i=1; i<arc_cR; i++){
		ptr_aux1 = ms_aux.getVertex(table_right[i-1]-1);
		ptr_aux2 = ms_aux.getVertex(table_right[i]-1);
		ptr_val1 = ms.getVertex(table_right[i-1]-1);
		ptr_val2 = ms.getVertex(table_right[i]-1);
		arc_length_right += sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
	}
	double d_lambda = (-Az) / (pow(tan(M_PI*0.5 - betaTransf),2) + 1.0);
	double d_shift = 0.5 * (r_m*r_m + HH*HH)/HH - HH;
	double c_auxX = (d_lambda*(-tan(-betaTransf+(M_PI*0.5))));
	double c_auxY = d_shift;
	double c_auxZ = -d_lambda;

	//centerD.x = meanX + h_aux*ddx; centerD.y = meanY + h_aux*ddy; centerD.z = meanZ + h_aux*ddz;
	centerD.x = c_auxX; centerD.y = c_auxY; centerD.z = c_auxZ;
	
	d_lambda = (-Az) / (pow(tan(M_PI*0.5 - alphaTransf),2) + 1.0);
	c_auxX = (d_lambda*(tan(-alphaTransf+(M_PI*0.5))));
	c_auxY = d_shift;
	c_auxZ = -d_lambda;

	//centerE.x = (meanX + h_aux*ddx); centerE.y = meanY + h_aux*ddy; centerE.z = meanZ + h_aux*ddz;
	centerE.x = c_auxX; centerE.y = c_auxY; centerE.z = c_auxZ;

	int s_count=0;
	double a1, a2, a3;
	int fail_counter = 0;
	FVPoint3D<double> v_point;
	
	if(left_or_right==0){ //transform right to left
	
		for(int i=0; i<num_vertex; i++)
		{
			ptr_v = ms_aux.getVertex(i);
			ptr_val = ms.getVertex(i);
			//dist_s1=1e20,dist_s2=1e21,dist_s3=1e22, dist_d1=1e20,dist_d2=1e21;
			//s1=-1,s2=-1,s3=-1, d1=-1,d2=-1;
			bool complete = false;
			//double theta = 0.0f;
			if((ptr_v->code==code_SutureRight) || (ptr_v->code==code_doubleSutureRight)) {
				//find 3 closest vertexes
				double vx=ptr_val->coord.x, vy=ptr_val->coord.y, vz=ptr_val->coord.z;
				double tau_arc, tau_dist;
				double v_dist = sqrt(pow(vx-pB.x,2) + pow(vy-pB.y,2) + pow(vz-pB.z,2));
				double v_angle = acos( ((pA.x-pB.x)*(vx-pB.x) + (pA.y-pB.y)*(vy-pB.y) + (pA.z-pB.z)*(vz-pB.z)) / (ab_dist*v_dist) );
				double v_arc_dist = 0.0;
				double parc_dist = 0.0;
				if(v_angle < angle_right[0]){
					ptr_aux1 = ms_aux.getVertex(table_right[0]-1);
					ptr_val1 = ms.getVertex(table_right[0]-1);
					v_point.x = pA.x + (v_angle/angle_right[0]) * (ptr_val1->coord.x-pA.x);
					v_point.y = pA.y + (v_angle/angle_right[0]) * (ptr_val1->coord.y-pA.y);
					v_point.z = pA.z + (v_angle/angle_right[0]) * (ptr_val1->coord.z-pA.z);
					v_arc_dist = sqrt(pow(v_point.x-pA.x,2) + pow(v_point.y-pA.y,2) + pow(v_point.z-pA.z,2));
					parc_dist = sqrt(pow(v_point.x-pB.x,2) + pow(v_point.y-pB.y,2) + pow(v_point.z-pB.z,2));
				}
				else{
					ptr_aux1 = ms_aux.getVertex(table_right[0]-1);
					ptr_val1 = ms.getVertex(table_right[0]-1);
					v_arc_dist += sqrt(pow(ptr_val1->coord.x-pA.x,2) + pow(ptr_val1->coord.y-pA.y,2) + pow(ptr_val1->coord.z-pA.z,2));
					bool ja_acabou = false;
					for(int tablei=1; (tablei<arc_cR) && (!ja_acabou); tablei++){
						if( v_angle > (angle_right[tablei] + 1e-8)){
							ptr_aux1 = ms_aux.getVertex(table_right[tablei-1]-1);
							ptr_aux2 = ms_aux.getVertex(table_right[tablei]-1);
							ptr_val1 = ms.getVertex(table_right[tablei-1]-1);
							ptr_val2 = ms.getVertex(table_right[tablei]-1);
							v_arc_dist += sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
						}
						else{
							ptr_aux1 = ms_aux.getVertex(table_right[tablei-1]-1);
							ptr_aux2 = ms_aux.getVertex(table_right[tablei]-1);
							ptr_val1 = ms.getVertex(table_right[tablei-1]-1);
							ptr_val2 = ms.getVertex(table_right[tablei]-1);
							v_point.x = ptr_val1->coord.x + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (ptr_val2->coord.x-ptr_val1->coord.x);
							v_point.y = ptr_val1->coord.y + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (ptr_val2->coord.y-ptr_val1->coord.y);
							v_point.z = ptr_val1->coord.z + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (ptr_val2->coord.z-ptr_val1->coord.z);
							v_arc_dist += sqrt(pow(v_point.x-ptr_val1->coord.x,2) + pow(v_point.y-ptr_val1->coord.y,2) + pow(v_point.z-ptr_val1->coord.z,2));
							ja_acabou = true;
							parc_dist = sqrt(pow(v_point.x-pB.x,2) + pow(v_point.y-pB.y,2) + pow(v_point.z-pB.z,2));
						}
					}
				}
			
				tau_arc = v_arc_dist / arc_length_right;
				if(parc_dist==0) tau_dist = 0.0f;
				else tau_dist = v_dist / parc_dist;

				//get vertex coordinates on the left side of the breast
				//arc_length_left -> left arc size
				double new_v_arc_dist = tau_arc * arc_length_left;
				double left_arc_dist;
				ptr_aux1 = ms_aux.getVertex(table_left[0]-1);
				ptr_val1 = ms.getVertex(table_left[0]-1);
				left_arc_dist = sqrt(pow(pA.x-ptr_val1->coord.x,2) + pow(pA.y-ptr_val1->coord.y,2) + pow(pA.z-ptr_val1->coord.z,2));
			
				if(new_v_arc_dist < left_arc_dist){
					ptr_aux1 = ms_aux.getVertex(table_left[0]-1);
					ptr_val1 = ms.getVertex(table_left[0]-1);
					v_point.x = pA.x + (new_v_arc_dist/left_arc_dist) * (ptr_val1->coord.x-pA.x);
					v_point.y = pA.y + (new_v_arc_dist/left_arc_dist) * (ptr_val1->coord.y-pA.y);
					v_point.z = pA.z + (new_v_arc_dist/left_arc_dist) * (ptr_val1->coord.z-pA.z);
				}
				else{
					new_v_arc_dist -= left_arc_dist;
					bool ja_acabou = false;
					for(int tablei=1; (tablei<arc_cL) && (!ja_acabou); tablei++){
						ptr_aux1 = ms_aux.getVertex(table_left[tablei-1]-1);
						ptr_aux2 = ms_aux.getVertex(table_left[tablei]-1);
						ptr_val1 = ms.getVertex(table_left[tablei-1]-1);
						ptr_val2 = ms.getVertex(table_left[tablei]-1);
						left_arc_dist = sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
						if( new_v_arc_dist < (left_arc_dist + 1e-8)){
							v_point.x = ptr_val1->coord.x + (new_v_arc_dist/left_arc_dist) * (ptr_val2->coord.x-ptr_val1->coord.x);
							v_point.y = ptr_val1->coord.y + (new_v_arc_dist/left_arc_dist) * (ptr_val2->coord.y-ptr_val1->coord.y);
							v_point.z = ptr_val1->coord.z + (new_v_arc_dist/left_arc_dist) * (ptr_val2->coord.z-ptr_val1->coord.z);
							ja_acabou = true;
						}
						else{
							new_v_arc_dist -= left_arc_dist;
						}
					}
				}
			
				vx = pB.x + tau_dist*(v_point.x-pB.x);
				vy = pB.y + tau_dist*(v_point.y-pB.y);
				vz = pB.z + tau_dist*(v_point.z-pB.z);
			
				double diff_test1 = sqrt(pow(centerE.x-vx,2)+pow(centerE.y-vy,2)+pow(centerE.z-vz,2));
				double diff_test2 = sqrt(pow(centerE.x-pE.x,2)+pow(centerE.y-pE.y,2)+pow(centerE.z-pE.z,2));
			
				double is_on_plane = Az + tan(M_PI*0.5 - alphaTransf)*vx - vz;
				//double is_on_plane = Az + tan(M_PI*0.5 - alphaTransf)*wX - wZ;
			
				complete = false;
				bool in_AB;
				std::vector< std::pair<double, int> > lista;
				lista.resize(0);
				for(int lit=0; lit<num_vertex; lit++)
				{
					ptr_aux1 = ms_aux.getVertex(lit);
					ptr_val1 = ms.getVertex(lit);
					if(((ptr_aux1->coord.x > (0.0-1e-8)) && (ptr_aux1->coord.x < (0.0+1e-8))) && ((ptr_aux1->coord.z > (Az-1e-8)) && (ptr_aux1->coord.z < (Az+1e-8))) ) in_AB = true;
					else in_AB = false;
					double v_p_dist1 = sqrt(pow(vx-ptr_val1->coord.x,2) + pow(vy-ptr_val1->coord.y,2) + pow(vz-ptr_val1->coord.z,2));
					if(((ptr_aux1->code==code_SutureLeft) || (ptr_aux1->code==code_doubleSutureLeft) || (in_AB)))
					{
						lista.push_back(std::make_pair(v_p_dist1, (int)ptr_aux1->label));
					}
					
				}
				int tamanho_lista = lista.size();
				std::sort(lista.begin(), lista.end(), pairCompare);
				for(int vamosver=0; vamosver<tamanho_lista; vamosver++){
					ptr_auxlit = ms.getVertex(lista[vamosver].second-1);
				}
				ptr_aux1 = ms_aux.getVertex(lista[0].second-1);
				ptr_val1 = ms.getVertex(lista[0].second-1);
				ptr_aux2 = ms_aux.getVertex(lista[1].second-1);
				ptr_val2 = ms.getVertex(lista[1].second-1);
				
				bool ja_ta=false;
				for(int new_lit=2; (new_lit<tamanho_lista) && (ja_ta==false); new_lit++)
				{
					ptr_aux3 = ms_aux.getVertex(lista[new_lit].second-1);
					ptr_val3 = ms.getVertex(lista[new_lit].second-1);
					double at = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at1 = area_tri(vx, vy, vz, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at2 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, vx, vy, vz, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at3 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, vx, vy, vz );
					a1 = at1/at;
					a2 = at2/at;
					a3 = at3/at;
					double a_soma = a1+a2+a3;
					a1 = a1/a_soma;
					a2 = a2/a_soma;
					a3 = a3/a_soma;
					
					if((a1>=0)&&(a2>=0)&&(a3>=0) && (((a1+a2+a3)>=(1.0-1e-8)) && ((a1+a2+a3)<=(1.0+1e-8))))
					{
						sSut[s_count] = ptr_v->label;
						sSutLabels[s_count*3] = ptr_aux1->label; sSutLabels[s_count*3+1] = ptr_aux2->label; sSutLabels[s_count*3+2] = ptr_aux3->label;
						sCoefs[s_count*3] = a1; sCoefs[s_count*3+1] = a2; sCoefs[s_count*3+2] = a3;
						s_count++;
						complete=true;
						ja_ta=true;
					}
				}
				
				if(ja_ta==false){
					ptr_aux1 = ms_aux.getVertex(lista[1].second-1);
					ptr_val1 = ms.getVertex(lista[1].second-1);
					ptr_aux2 = ms_aux.getVertex(lista[2].second-1);
					ptr_val2 = ms.getVertex(lista[2].second-1);
				
					for(int new_lit=3; (new_lit<tamanho_lista) && (ja_ta==false); new_lit++)
					{
						ptr_aux3 = ms_aux.getVertex(lista[new_lit].second-1);
						ptr_val3 = ms.getVertex(lista[new_lit].second-1);
						double at = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at1 = area_tri(vx, vy, vz, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at2 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, vx, vy, vz, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at3 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, vx, vy, vz );
						a1 = at1/at;
						a2 = at2/at;
						a3 = at3/at;
						double a_soma = a1+a2+a3;
						a1 = a1/a_soma;
						a2 = a2/a_soma;
						a3 = a3/a_soma;
					
						if((a1>=0)&&(a2>=0)&&(a3>=0) && (((a1+a2+a3)>=(1.0-1e-8)) && ((a1+a2+a3)<=(1.0+1e-8))))
						{
							sSut[s_count] = ptr_v->label;
							sSutLabels[s_count*3] = ptr_aux1->label; sSutLabels[s_count*3+1] = ptr_aux2->label; sSutLabels[s_count*3+2] = ptr_aux3->label;
							sCoefs[s_count*3] = a1; sCoefs[s_count*3+1] = a2; sCoefs[s_count*3+2] = a3;
							s_count++;
							complete=true;
							ja_ta=true;
						}
					}
				}
				if(complete==false) {printf("\nN\ao encontrou match\n\n\n\n\n\n"); fail_counter++;}
			}
		}
	}
	
	//transform left to right
	else{
	
		for(int i=0; i<num_vertex; i++)
		{
			ptr_v = ms_aux.getVertex(i);
			ptr_val = ms.getVertex(i);
			//dist_s1=1e20,dist_s2=1e21,dist_s3=1e22, dist_d1=1e20,dist_d2=1e21;
			//s1=-1,s2=-1,s3=-1, d1=-1,d2=-1;
			bool complete = false;
			//double theta = 0.0f;
			if((ptr_v->code==code_SutureLeft) || (ptr_v->code==code_doubleSutureLeft)) {
				//find 3 closest vertexes
				double vx=ptr_val->coord.x, vy=ptr_val->coord.y, vz=ptr_val->coord.z;
			
				double tau_arc, tau_dist;
				double v_dist = sqrt(pow(vx-pB.x,2) + pow(vy-pB.y,2) + pow(vz-pB.z,2));
				double v_angle = acos( ((pA.x-pB.x)*(vx-pB.x) + (pA.y-pB.y)*(vy-pB.y) + (pA.z-pB.z)*(vz-pB.z)) / (ab_dist*v_dist) );
				double v_arc_dist = 0.0;
				double parc_dist = 0.0;
				if(v_angle < angle_left[0]){
					ptr_aux1 = ms_aux.getVertex(table_left[0]-1);
					ptr_val1 = ms.getVertex(table_left[0]-1);
					v_point.x = pA.x + (v_angle/angle_left[0]) * (ptr_val1->coord.x-pA.x);
					v_point.y = pA.y + (v_angle/angle_left[0]) * (ptr_val1->coord.y-pA.y);
					v_point.z = pA.z + (v_angle/angle_left[0]) * (ptr_val1->coord.z-pA.z);
					v_arc_dist = sqrt(pow(v_point.x-pA.x,2) + pow(v_point.y-pA.y,2) + pow(v_point.z-pA.z,2));
					parc_dist = sqrt(pow(v_point.x-pB.x,2) + pow(v_point.y-pB.y,2) + pow(v_point.z-pB.z,2));
				}
				else{
					ptr_aux1 = ms_aux.getVertex(table_left[0]-1);
					ptr_val1 = ms.getVertex(table_left[0]-1);
					v_arc_dist += sqrt(pow(ptr_val1->coord.x-pA.x,2) + pow(ptr_val1->coord.y-pA.y,2) + pow(ptr_val1->coord.z-pA.z,2));
					bool ja_acabou = false;
					for(int tablei=1; (tablei<arc_cL) && (!ja_acabou); tablei++){
						if( v_angle > (angle_left[tablei] + 1e-8)){
							ptr_aux1 = ms_aux.getVertex(table_left[tablei-1]-1);
							ptr_aux2 = ms_aux.getVertex(table_left[tablei]-1);
							ptr_val1 = ms.getVertex(table_left[tablei-1]-1);
							ptr_val2 = ms.getVertex(table_left[tablei]-1);
							v_arc_dist += sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
						}
						else{
							ptr_aux1 = ms_aux.getVertex(table_left[tablei-1]-1);
							ptr_aux2 = ms_aux.getVertex(table_left[tablei]-1);
							ptr_val1 = ms.getVertex(table_left[tablei-1]-1);
							ptr_val2 = ms.getVertex(table_left[tablei]-1);
							v_point.x = ptr_val1->coord.x + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (ptr_val2->coord.x-ptr_val1->coord.x);
							v_point.y = ptr_val1->coord.y + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (ptr_val2->coord.y-ptr_val1->coord.y);
							v_point.z = ptr_val1->coord.z + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (ptr_val2->coord.z-ptr_val1->coord.z);
							v_arc_dist += sqrt(pow(v_point.x-ptr_val1->coord.x,2) + pow(v_point.y-ptr_val1->coord.y,2) + pow(v_point.z-ptr_val1->coord.z,2));
							ja_acabou = true;
							parc_dist = sqrt(pow(v_point.x-pB.x,2) + pow(v_point.y-pB.y,2) + pow(v_point.z-pB.z,2));
						}
					}
				}
			
				tau_arc = v_arc_dist / arc_length_left;
				if(parc_dist==0) tau_dist = 0;
				else tau_dist = v_dist / parc_dist;

				//get vertex coordinates on the left side of the breast
				//arc_length_left -> left arc size
				double new_v_arc_dist = tau_arc * arc_length_right;
				double right_arc_dist;
				ptr_aux1 = ms_aux.getVertex(table_right[0]-1);
				ptr_val1 = ms.getVertex(table_right[0]-1);
				right_arc_dist = sqrt(pow(pA.x-ptr_val1->coord.x,2) + pow(pA.y-ptr_val1->coord.y,2) + pow(pA.z-ptr_val1->coord.z,2));
			
				if(new_v_arc_dist < right_arc_dist){
					ptr_aux1 = ms_aux.getVertex(table_right[0]-1);
					ptr_val1 = ms.getVertex(table_right[0]-1);
					v_point.x = pA.x + (new_v_arc_dist/right_arc_dist) * (ptr_val1->coord.x-pA.x);
					v_point.y = pA.y + (new_v_arc_dist/right_arc_dist) * (ptr_val1->coord.y-pA.y);
					v_point.z = pA.z + (new_v_arc_dist/right_arc_dist) * (ptr_val1->coord.z-pA.z);
				}
				else{
					new_v_arc_dist -= right_arc_dist;
					bool ja_acabou = false;
					for(int tablei=1; (tablei<arc_cR) && (!ja_acabou); tablei++){
						ptr_aux1 = ms_aux.getVertex(table_right[tablei-1]-1);
						ptr_aux2 = ms_aux.getVertex(table_right[tablei]-1);
						ptr_val1 = ms.getVertex(table_right[tablei-1]-1);
						ptr_val2 = ms.getVertex(table_right[tablei]-1);
						right_arc_dist = sqrt(pow(ptr_val1->coord.x - ptr_val2->coord.x,2)+pow(ptr_val1->coord.y - ptr_val2->coord.y,2)+pow(ptr_val1->coord.z - ptr_val2->coord.z,2));
						if( new_v_arc_dist < (right_arc_dist + 1e-8)){
							v_point.x = ptr_val1->coord.x + (new_v_arc_dist/right_arc_dist) * (ptr_val2->coord.x-ptr_val1->coord.x);
							v_point.y = ptr_val1->coord.y + (new_v_arc_dist/right_arc_dist) * (ptr_val2->coord.y-ptr_val1->coord.y);
							v_point.z = ptr_val1->coord.z + (new_v_arc_dist/right_arc_dist) * (ptr_val2->coord.z-ptr_val1->coord.z);
							ja_acabou = true;
						}
						else{
							new_v_arc_dist -= right_arc_dist;
						}
					}
				}
			
				vx = pB.x + tau_dist*(v_point.x-pB.x);
				vy = pB.y + tau_dist*(v_point.y-pB.y);
				vz = pB.z + tau_dist*(v_point.z-pB.z);
			
				double diff_test1 = sqrt(pow(centerD.x-vx,2)+pow(centerD.y-vy,2)+pow(centerD.z-vz,2));
				double diff_test2 = sqrt(pow(centerD.x-pD.x,2)+pow(centerD.y-pE.y,2)+pow(centerD.z-pD.z,2));
			
				double is_on_plane = Az + tan(M_PI*0.5 - betaTransf)*vx - vz;
				//double is_on_plane = Az + tan(M_PI*0.5 - alphaTransf)*wX - wZ;
			
				complete = false;
				bool in_AB;
				std::vector< std::pair<double, int> > lista;
				lista.resize(0);
				for(int lit=0; lit<num_vertex; lit++)
				{
					ptr_aux1 = ms_aux.getVertex(lit);
					ptr_val1 = ms.getVertex(lit);
					if(((ptr_aux1->coord.x > (0.0-1e-8)) && (ptr_aux1->coord.x < (0.0+1e-8))) && ((ptr_aux1->coord.z > (Az-1e-8)) && (ptr_aux1->coord.z < (Az+1e-8))) ) in_AB = true;
					else in_AB = false;
					double v_p_dist1 = sqrt(pow(vx-ptr_val1->coord.x,2) + pow(vy-ptr_val1->coord.y,2) + pow(vz-ptr_val1->coord.z,2));
					if(((ptr_aux1->code==code_SutureRight) || (ptr_aux1->code==code_doubleSutureRight) || (in_AB)))
					{
						lista.push_back(std::make_pair(v_p_dist1, (int)ptr_aux1->label));
					}
					
				}
				int tamanho_lista = lista.size();
				std::sort(lista.begin(), lista.end(), pairCompare);
				for(int vamosver=0; vamosver<tamanho_lista; vamosver++){
					ptr_auxlit = ms.getVertex(lista[vamosver].second-1);
				}
				ptr_aux1 = ms_aux.getVertex(lista[0].second-1);
				ptr_val1 = ms.getVertex(lista[0].second-1);
				ptr_aux2 = ms_aux.getVertex(lista[1].second-1);
				ptr_val2 = ms.getVertex(lista[1].second-1);
				
				bool ja_ta=false;
				for(int new_lit=2; (new_lit<tamanho_lista) && (ja_ta==false); new_lit++)
				{
					ptr_aux3 = ms_aux.getVertex(lista[new_lit].second-1);
					ptr_val3 = ms.getVertex(lista[new_lit].second-1);
					double at = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at1 = area_tri(vx, vy, vz, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at2 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, vx, vy, vz, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
					double at3 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, vx, vy, vz );
					a1 = at1/at;
					a2 = at2/at;
					a3 = at3/at;
					double a_soma = a1+a2+a3;
					a1 = a1/a_soma;
					a2 = a2/a_soma;
					a3 = a3/a_soma;
					
					if((a1>=0)&&(a2>=0)&&(a3>=0) && (((a1+a2+a3)>=(1.0-1e-8)) && ((a1+a2+a3)<=(1.0+1e-8))))
					{
						sSut[s_count] = ptr_v->label;
						sSutLabels[s_count*3] = ptr_aux1->label; sSutLabels[s_count*3+1] = ptr_aux2->label; sSutLabels[s_count*3+2] = ptr_aux3->label;
						sCoefs[s_count*3] = a1; sCoefs[s_count*3+1] = a2; sCoefs[s_count*3+2] = a3;
						s_count++;
						complete=true;
						ja_ta=true;
					}
				}
				
				if(ja_ta==false){
					ptr_aux1 = ms_aux.getVertex(lista[1].second-1);
					ptr_val1 = ms.getVertex(lista[1].second-1);
					ptr_aux2 = ms_aux.getVertex(lista[2].second-1);
					ptr_val2 = ms.getVertex(lista[2].second-1);
				
					bool ja_ta=false;
					for(int new_lit=3; (new_lit<tamanho_lista) && (ja_ta==false); new_lit++)
					{
						ptr_aux3 = ms_aux.getVertex(lista[new_lit].second-1);
						ptr_val3 = ms.getVertex(lista[new_lit].second-1);
						double at = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at1 = area_tri(vx, vy, vz, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at2 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, vx, vy, vz, ptr_val3->coord.x, ptr_val3->coord.y, ptr_val3->coord.z );
						double at3 = area_tri(ptr_val1->coord.x, ptr_val1->coord.y, ptr_val1->coord.z, ptr_val2->coord.x, ptr_val2->coord.y, ptr_val2->coord.z, vx, vy, vz );
						a1 = at1/at;
						a2 = at2/at;
						a3 = at3/at;
						
						double a_soma = a1+a2+a3;
						a1 = a1/a_soma;
						a2 = a2/a_soma;
						a3 = a3/a_soma;
					
						if((a1>=0)&&(a2>=0)&&(a3>=0) && (((a1+a2+a3)>=(1.0-1e-8)) && ((a1+a2+a3)<=(1.0+1e-8))))
						{
							sSut[s_count] = ptr_v->label;
							sSutLabels[s_count*3] = ptr_aux1->label; sSutLabels[s_count*3+1] = ptr_aux2->label; sSutLabels[s_count*3+2] = ptr_aux3->label;
							sCoefs[s_count*3] = a1; sCoefs[s_count*3+1] = a2; sCoefs[s_count*3+2] = a3;
							s_count++;
							complete=true;
							ja_ta=true;
						}
					}
				}
				if(complete==false) {printf("\nN\ao encontrou match\n\n\n\n\n\n"); fail_counter++;}
			}
		}
	}

delete [] table_left;
delete [] table_right;
delete [] angle_left;
delete [] angle_right;


initialize(ms,p2,WMat2);
cout<<"initialisation done"<<endl;fflush(NULL);

static double NormEpsilon2=0.,MaxIter2=0.;
static bool no_cond2=true;
if(no_cond2)
    { 
    NormEpsilon2=para.getDouble("NormEpsilon");
    MaxIter2=para.getDouble("MaxIter");
    no_cond2=false;
    }
dados2XXX(ms,ms_aux,p2,para);
CalcGrad(ms,ms_aux,p2,W2,WS2,WMat2,para);
for (int i=0; i<NNN; i++) {
 P2[i]=-RRR[i]; 
}

for (int i=0; i<NNN; i++) {
 XXX1[i]=XXX[i];
}
//LineSearch1(ms,ms_aux,p2,W2,WS2,WMat2,para);
LineSearch_alt1(ms,ms_aux,p2,W2,WS2,WMat2,para);
for (int i=0; i<NNN; i++) {
 XXX2[i]=XXX1[i]+alpha*P2[i];
}

nor1=0;
for (int i=0; i<NNN; i++) {
 nor1+=RRR[i]*RRR[i];
}

for (int i=0; i<NNN; i++) {
 P1[i]=P2[i];
 XXX1[i]=XXX2[i];
}
cout<<"gradient done"<<endl;
// the conjugate gradient 
size_t count2=0,nb_iter2=0;
//while ( (nor1 > NormEpsilon2) && (nb_iter2< MaxIter2) ) 
double act_energy = 10000000000.0f;
double energy_variation = 1e-5;
double global_energy = JJJ;
while( (fabs(act_energy - global_energy) > energy_variation) && (nb_iter2<1000))
{  //while
    nb_iter2++;
    act_energy = global_energy;
    for (int i=0; i<NNN; i++) 
        {
        XXX[i]=XXX1[i];
        }
    for (int i=0; i<NNN; i++) 
        {
        RRR1[i]=RRR[i];
        }
    CalcGrad(ms,ms_aux,p2,W2,WS2,WMat2,para);

    nor2=0;
    for (int i=0; i<NNN; i++) 
        {
        nor2+=RRR[i]*RRR[i];
        }
    nor3=0;
    for (int i=0; i<NNN; i++) 
        {
        nor3+=RRR[i]*RRR1[i];
        }
    beta=(nor2-nor3)/nor1;
    nor1=nor2;
    count2++;
    if ((int)count2>NNN) 
        {
        count2=0;
        beta=0;
        }
    for (int i=0; i<NNN; i++) 
        {
        P2[i]=-RRR[i]+beta*P1[i];
        }
    for (int i=0; i<NNN; i++)
        {
        XXX[i]=XXX1[i]; 
        }
    //LineSearch1(ms,ms_aux,p2,W2,WS2,WMat2,para);
    LineSearch_alt1(ms,ms_aux,p2,W2,WS2,WMat2,para);
    for (int i=0; i<NNN; i++) 
        {
        XXX2[i]=XXX1[i]+alpha*P2[i];
        }
    for (int i=0; i<NNN; i++) 
        {
        P1[i]=P2[i];
        XXX1[i]=XXX2[i];
        }
        
        global_energy = JJJ;
        
		int auxN = 0;
		ms.beginVertex();
       {
    	string cfilename = string("percentage.txt");
		ofstream  counter_file;    
		counter_file.open(cfilename.c_str());
		double percentage = ((double)nb_iter2)/((double)MaxIter2) * 100;
		printf("\nPercentage: %f\n", percentage);
        counter_file << percentage <<endl;
        counter_file.close(); 
        //FVVertex3D *ptr_v;
		//NNN=0;
    	}
       
       
    //printf("iteration %lu  with residual %8.5e <-> energy_variation:%e    \n",count2,nor1,fabs(act_energy - global_energy));    fflush(NULL);
	}//end of while
printf("\n \n final iteration %lu  with residual %8.5e  <-> energy_variation:%e     \n",count2,nor1,fabs(act_energy - global_energy));    fflush(NULL);  
double skin_val = 0.002;
XXX2dados(ms,ms_aux,p2,para);
eval_teste(ms,ms_aux,p2,W2,WS2,WMat2,para, skin_val);
XXX2dados(ms,ms_aux,p2,para); // return to the C++ world
cout<<" found "<<ms.getNbVertex()<<" vertices, "<<ms.getNbCell()<<" cells, "
               <<ms.getNbFace()<<" faces."<<endl;

	string cfilename = string("percentage.txt");
	ofstream  counter_file;    
	counter_file.open(cfilename.c_str());
	double percentage = 100.0f;
    counter_file << percentage <<endl;
    counter_file.close();
//
//
//
//save the final  position

write_mesh(ms, p2, para);


string displacement_filename,position_filename;
position_filename=para.getString("SuturedPositionFile");
FVio position_file(position_filename.c_str(),FVWRITE);
position_file.put(p2,0.0,"position"); 
// compute the displacement
{
FVVertex3D *ptr_v;
ms.beginVertex();
while ((ptr_v=ms.nextVertex()))
    {
    size_t i=ptr_v->label-1;    
    u2[i]=p2[i]-ptr_v->coord;
    }
}
//save the final displacement
displacement_filename=para.getString("SuturedDisplacementFile");
FVio displacement_file(displacement_filename.c_str(),FVWRITE);
displacement_file.put(u2,0.0,"displacement"); 

}