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

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

#define DIM 10000
double XXX[DIM], XXX1[DIM], XXX2[DIM];
double VVV[DIM];
double P1[DIM], P2[DIM];
double  RRR[DIM], RRR1[DIM];
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;
int sutureP[DIM];
int numSutureP;
double sCoefs[DIM*3], dCoefs[DIM*2]; 
int numSutureVertex;
double leftVals[DIM];// rightVals[DIM];
double JJJ=0, JJJ0=0;
double nor1, nor2, nor3;
int NNN=4;
double alpha=0, beta=0;
size_t CodeRemove=1000;
double norma=100000; 
double epsilon=0.001;
int num_arc_L;
int num_arc_R;

int global_it;




//-----------------------------//
//-----------------------------//
//-----------------------------//
//-----------------------------//
//-----------------------------//
//-----------------------------//
//-----------------------------//
//Displacement functions


void dados2XXX_disp(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,Parameter &para)
{   
size_t i;  
static size_t code_fixe,code_chassignac,code_leftSuture,code_rightSuture,code_bottomSuture;
static size_t 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_bottomSuture=para.getUnsigned("CodeBottomSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
    no_cond=false;
   }
FVVertex3D *ptr_v;
NNN=0;
ms.beginVertex();
//printf("\nINICIO dados2xxx\n");
while((ptr_v=ms.nextVertex()))
    { 
    	i=ptr_v->label-1; 
    	if(ptr_v->code==code_fixe) continue;
    	if(ptr_v->code==code_chassignac)
        {
         //XXX[NNN++]=p[i].x;
         XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z;
         continue;
        }  
    	if((ptr_v->code==code_doubleSutureLeft))
        {
        	if(left_or_right==0){
         		//XXX[NNN++]=p[i].y=0;
         		XXX[NNN++]=p[i].x;
         		//XXX[NNN++]=p[i].y;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        }       
    	if((ptr_v->code==code_doubleSutureRight))
        {
        	if(left_or_right==1){
         		//XXX[NNN++]=p[i].y=0;
         		XXX[NNN++]=p[i].x;
         		//XXX[NNN++]=p[i].y;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        } 
        if((ptr_v->code==code_leftSuture))
        {
        	//printf("\nLeft SUP\n");
        	if(left_or_right==0){
         		//XXX[NNN++]=p[i].y=0;
         		XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z;
         	}
         continue;
        }   
    	if((ptr_v->code==code_rightSuture))
        {
        	//printf("\nRight SUP\n");
        	if(left_or_right==1){
         		//XXX[NNN++]=p[i].y=0;
         		XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z;
         	}
         continue;
        }  
    	if(ptr_v->code==code_bottomSuture)
        {
         //XXX[NNN++]=p[i].y=0;
         XXX[NNN++]=p[i].x;XXX[NNN++]=p[i].z;
         continue;
        }
     	XXX[NNN++]=p[i].x;XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z; 
     }
//for(i=0;i<(size_t)NNN;i++) cout<<"XXX[i]="<<XXX[i]<<endl;    
}

void XXX2dados_disp(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,Parameter &para)
{    
size_t i;  
static size_t code_fixe,code_chassignac,code_leftSuture,code_rightSuture,code_bottomSuture;
static size_t 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_bottomSuture=para.getUnsigned("CodeBottomSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");       
    no_cond=false;
   } 

FVVertex3D *ptr_v;//, *ptr_1, *ptr_2, *ptr_3;
NNN=0;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {
    i=ptr_v->label-1;          
    if(ptr_v->code==code_fixe) continue;
    if(ptr_v->code==code_chassignac) 
        {
        //XXX[NNN++];
        p[i].y=XXX[NNN++];p[i].z=XXX[NNN++];  
        continue;
        } 
    if((ptr_v->code==code_doubleSutureLeft)) 
        {
        	if(left_or_right==0){
        		//p[i].y=0;p[i].x=0.;p[i].z=XXX[NNN++]; 
        		p[i].y=0;p[i].x=XXX[NNN++];p[i].z=XXX[NNN++];  
        	}
        continue;
        }   
    if((ptr_v->code==code_doubleSutureRight)) 
        {
        	if(left_or_right==1){
        		//p[i].y=0;p[i].x=0.;p[i].z=XXX[NNN++]; 
        		p[i].y=0;p[i].x=XXX[NNN++];p[i].z=XXX[NNN++];   
        	}
        continue;
        }  
    if((ptr_v->code==code_leftSuture))
        {
        	if(left_or_right==0){
        		//p[i].x=0;
        		p[i].x=XXX[NNN++];
        		p[i].y=XXX[NNN++];p[i].z=XXX[NNN++];
        	}
         continue;
        }
    if((ptr_v->code==code_rightSuture))
        {
        	if(left_or_right==1){
        		//p[i].x=0;
        		p[i].x=XXX[NNN++];
        		p[i].y=XXX[NNN++];p[i].z=XXX[NNN++]; 
        	}
        continue;            
        }          
    if(ptr_v->code==code_bottomSuture) 
        {
        p[i].y=0;
        p[i].x=XXX[NNN++];p[i].z=XXX[NNN++];  
        continue;
        }     
    p[i].x=XXX[NNN++];p[i].y=XXX[NNN++];p[i].z=XXX[NNN++];
    }
    
    //int sSut[DIM], dSut[DIM], sSutLabels[DIM*3], dSutLabels[DIM*2];
	//int num_sSuture, num_dSuture;
	//double sCoefs[DIM*3], dCoefs[DIM*2]; 
    
    //use table relation between vertex-line for doublesuture vertexes
    //Px = a1Q1x + a2Q2x;
    //Py = 0;
    //Pz = a1Q1z + a2Q2z;
    
    //use table relation between vertex-triangle for suture vertexes
    //Px = a1Q1x + a2Q2x + a3Q3x;
    //Py = a1Q1y + a2Q2y + a3Q3y;
    //Pz = a1Q1z + a2Q2z + a3Q3z;
    
    int num_vertex = ms.getNbVertex();
    if(left_or_right==0){ //transform right to left
    
    	for(int k=0; k<num_vertex; k++)
    	{
    		ptr_v=ms.getVertex(k);
    		i=ptr_v->label-1;
    		if(ptr_v->code==code_rightSuture)
        	{
        		int ptrlabel = ptr_v->label;
        		//printf("\nlabel dir:%d\n", ptrlabel);
        		for(int it=0; it<num_sSuture; it++)
        		{
        			if(sSut[it]==ptrlabel)
        			{
        				//ptr_1 = ms.getVertex(sSutLabels[it*3]-1);
        				//ptr_2 = ms.getVertex(sSutLabels[it*3+1]-1);
        				//ptr_3 = ms.getVertex(sSutLabels[it*3+2]-1);
        				//double antx = p[i].x, anty = p[i].y, antz = p[i].z;
        				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;
        				//if(i==390) printf("\np[i] %e,%e,%e -> %e,%e,%e\n", antx, anty, antz, p[i].x, p[i].y, p[i].z);
        				//if(i==390) printf("\nptr1 -> %e,%e,%e -- ptr2 -> %e,%e,%e -- ptr3 -> %e,%e,%e\n", ptr_1->coord.x, ptr_1->coord.y, ptr_1->coord.z, ptr_2->coord.x, ptr_2->coord.y, ptr_2->coord.z, ptr_3->coord.x, ptr_3->coord.y, ptr_3->coord.z );
        			}
        		}           
        	}
        	else if(ptr_v->code==code_doubleSutureRight)
        	{
        		int ptrlabel = ptr_v->label;
        		//printf("\nNum dsuture:%d\n", num_dSuture);
        		for(int it=0; it<num_sSuture; it++)
        		{
        			if(sSut[it]==ptrlabel)
        			{
        				//ptr_1 = ms.getVertex(sSutLabels[it*3]-1);
        				//ptr_2 = ms.getVertex(sSutLabels[it*3+1]-1);
        				//ptr_3 = ms.getVertex(sSutLabels[it*3+2]-1);
        				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;
        				//if(i==390) printf("\np[i] -> %e,%e,%e\n", p[i].x, p[i].y, p[i].z);
        				//if(i==390) printf("\nptr1 -> %e,%e,%e -- ptr2 -> %e,%e,%e -- ptr3 -> %e,%e,%e\n", ptr_1->coord.x, ptr_1->coord.y, ptr_1->coord.z, ptr_2->coord.x, ptr_2->coord.y, ptr_2->coord.z, ptr_3->coord.x, ptr_3->coord.y, ptr_3->coord.z );
        			}
        		}          
        	}  
    	}
    }
    else{ //transform left to right
    	for(int k=0; k<num_vertex; k++)
    	{
    		ptr_v=ms.getVertex(k);
    		i=ptr_v->label-1;
    		if(ptr_v->code==code_leftSuture)
        	{
        		int ptrlabel = ptr_v->label;
        		//printf("\nlabel dir:%d\n", ptrlabel);
        		for(int it=0; it<num_sSuture; it++)
        		{
        			if(sSut[it]==ptrlabel)
        			{
        				//ptr_1 = ms.getVertex(sSutLabels[it*3]-1);
        				//ptr_2 = ms.getVertex(sSutLabels[it*3+1]-1);
        				//ptr_3 = ms.getVertex(sSutLabels[it*3+2]-1);
        				//double antx = p[i].x, anty = p[i].y, antz = p[i].z;
        				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;
        				//if(i==390) printf("\np[i] %e,%e,%e -> %e,%e,%e\n", antx, anty, antz, p[i].x, p[i].y, p[i].z);
        				//if(i==390) printf("\nptr1 -> %e,%e,%e -- ptr2 -> %e,%e,%e -- ptr3 -> %e,%e,%e\n", ptr_1->coord.x, ptr_1->coord.y, ptr_1->coord.z, ptr_2->coord.x, ptr_2->coord.y, ptr_2->coord.z, ptr_3->coord.x, ptr_3->coord.y, ptr_3->coord.z );
        			}
        		}           
        	}
        	else if(ptr_v->code==code_doubleSutureLeft)
        	{
        		int ptrlabel = ptr_v->label;
        		//printf("\nNum dsuture:%d\n", num_dSuture);
        		for(int it=0; it<num_sSuture; it++)
        		{
        			if(sSut[it]==ptrlabel)
        			{
        				//ptr_1 = ms.getVertex(sSutLabels[it*3]-1);
        				//ptr_2 = ms.getVertex(sSutLabels[it*3+1]-1);
        				//ptr_3 = ms.getVertex(sSutLabels[it*3+2]-1);
        				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;
        				//if(i==390) printf("\np[i] -> %e,%e,%e\n", p[i].x, p[i].y, p[i].z);
        				//if(i==390) printf("\nptr1 -> %e,%e,%e -- ptr2 -> %e,%e,%e -- ptr3 -> %e,%e,%e\n", ptr_1->coord.x, ptr_1->coord.y, ptr_1->coord.z, ptr_2->coord.x, ptr_2->coord.y, ptr_2->coord.z, ptr_3->coord.x, ptr_3->coord.y, ptr_3->coord.z );
        			}
        		}          
        	}  
    	}
    }
    
//for(i=0;i<(size_t)NNN;i++) cout<<"XXX[i]="<<XXX[i]<<endl;
}





// -------- evaluation of the functional -----------------
double eval_disp(FVMesh3D &ms,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;
//cout<<"entering in eval"<<endl;fflush(NULL);
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];
    }
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]; 
        }
    }
    
    
    //cout<<"sum="<<sum<<endl;
return(sum);     
}
// -------- evaluation of the functional -----------------
double eval_local_disp(FVMesh3D &ms,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_doubleSutureLeft,code_doubleSutureRight, code_leftSuture,code_rightSuture,code_bottomSuture;
//static size_t code_dispRight, code_dispLeft;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    //code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    //code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");
    //code_dispLeft=para.getUnsigned("CodeDispLeft");
    //code_dispRight=para.getUnsigned("CodeDispRight");
    code_fixe=para.getUnsigned("CodeFixation");
    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;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    if(ptr_v->code==code_fixe) continue;
    if(ptr_v->code==code_chassignac) {j+=2;}
    else {j+=3;}    
    if(j>ind) break;   
    }
sum=initial_value;
ptr_v->beginCell();
while((ptr_c=ptr_v->nextCell()))
    { 
    //cout<<"cell "<<ptr_c->label<<endl;        
    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;
    //cout<<"val="<<val<<" vol="<<ptr_c->volume<<endl;
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume;           
    }

    //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 
           }   
        }   
    }    
// 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
        }
    }  
return(sum);     
}

//eval functional of energy when transformation occurs from the right side to the left
double eval_local_dispRight(FVMesh3D &ms,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_bottomSuture;
static size_t code_doubleSutureLeft, code_doubleSutureRight;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");  
    code_fixe=para.getUnsigned("CodeFixation");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    if(ptr_v->code==code_fixe) continue;
    else if((ptr_v->code==code_doubleSutureLeft)) {j+=2;}    
    else if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)) {j+=2;}
    else if((ptr_v->code==code_leftSuture)) {j+=3;}
    else if((ptr_v->code==code_rightSuture)||(ptr_v->code==code_doubleSutureRight)) {j+=0;}
    else /*if((ptr_v->code==0))*/{j+=3;}    
    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;
    double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}    			
    			}
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume; 
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	//int match_ind = -1;
	//int num_vertex = ms.getNbVertex();
	//double tdist = 1e20;
	//num_sSuture, num_dSuture;
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
	//if(num_dSuture>num_sSuture) num_s = num_dSuture;
    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;
    			double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    			sum=sum - W[ptr_c->label-1] + val*ptr_c->volume; 
    			//sum=sum - W[ptr_c->label-1] + val*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 
           }   
        }   
    }   

if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	//int match_ind = -1;
	//int num_vertex = ms.getNbVertex();
	//double tdist = 1e20;
	//num_sSuture, num_dSuture;
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
	//if(num_dSuture>num_sSuture) num_s = num_dSuture;
    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);
   			// printf("\nleft:%d -- right:%d\n", ptr_v->label, ptr_vv->label);  
			ptr_vv->beginCell();
			while((ptr_c=ptr_v->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==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	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
        	}
    	} 
    }
} 

//printf("\neval_local sum:%e\n", sum);

return(sum);     
}



//eval functional of energy when transformation occurs from the left side to the right
double eval_local_dispLeft(FVMesh3D &ms,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_bottomSuture;
static size_t code_doubleSutureRight, code_doubleSutureLeft;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");  
    code_fixe=para.getUnsigned("CodeFixation");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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);


//printf("\n1\n");
// search the vertex
size_t j=0;    
FVVertex3D *ptr_v, *ptr_vv;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    if(ptr_v->code==code_fixe) continue;
    else if((ptr_v->code==code_doubleSutureRight)) {j+=2;}
    else if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)) {j+=2;}
    else if((ptr_v->code==code_leftSuture)||(ptr_v->code==code_doubleSutureLeft)) {j+=0;}
    else if((ptr_v->code==code_rightSuture)) {j+=3;}
    else /*if((ptr_v->code==0))*/{j+=3;}    
    if(j>ind) break;   
    }
//printf("\n2\n");
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;
    double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume; 
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
if((ptr_v->code==code_rightSuture) || (ptr_v->code==code_doubleSutureRight)){
	//int match_ind = -1;
	//int num_vertex = ms.getNbVertex();
	//double tdist = 1e20;
	//num_sSuture, num_dSuture;
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
	//if(num_dSuture>num_sSuture) num_s = num_dSuture;
    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;
    			double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    			sum=sum - W[ptr_c->label-1] + val*ptr_c->volume; 
    			//sum=sum - W[ptr_c->label-1] + val*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;}
    }
    
}

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 
           }   
        }   
    }   

if((ptr_v->code==code_rightSuture) || (ptr_v->code==code_doubleSutureRight)){
	//int match_ind = -1;
	//int num_vertex = ms.getNbVertex();
	//double tdist = 1e20;
	//num_sSuture, num_dSuture;
	int num_s = num_sSuture;
	int ptrlabel = ptr_v->label;
	//if(num_dSuture>num_sSuture) num_s = num_dSuture;
    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_v->nextCell()))
    			{
    			ptr_c->beginFace();   
    			while((ptr_f=ptr_c->nextFace()))
        			{ 
        			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==code_rightSuture) || (ptr_v->code==code_doubleSutureRight)){
	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
        	}
    	} 
    }
} 

return(sum);     
}


///-----------------------------------///
///-----------------------------------///
///-----------------------------------///
///-----------------------------------///
///-----------------------------------///
//Cirurgia



double rho(double theta, double Az, double r_m){
	
	if((theta == 0 )) { return r_m+Az;} 
	else if(theta == M_PI){ return r_m-Az;}
	else {
	double B = theta - asin((-Az*sin(theta))/(r_m));
	return ((r_m*sin(B))/(sin(theta)));
	}
}

void makeDeformMesh(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p, Parameter &para)
{  
vector <FVVertex3D * > list_vertex(ms.getNbVertex());
vector <FVEdge3D * > list_edge(ms.getNbEdge());
vector <FVFace3D * > list_face(ms.getNbFace());
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"); 

// ---------  make the correspondance tables for face, edge and vertex -----------
/*
//int xak=0;//, xok=0;
    printf("\nFACE\n");
    ms.beginFace();
while((ptr_lf=ms.nextFace()))
    {
    list_face[ptr_lf->label-1]=ptr_lf;
    if(!((ptr_lf->code==code_SutureLeft) || (ptr_lf->code==code_doubleSutureLeft)))  continue;
   // printf("\nptr_lf->label:%d, coords:%f,%f,%f\n", (int)ptr_lf->label, ptr_lf->centroid.x, ptr_lf->centroid.y, ptr_lf->centroid.z);
    ptr_rf=NULL;dist=1.e10;
    //printf("\ncode:%d\n", ptr_lf->code);
    // we have a left side, fide the right side
    //int xok=0;
    for(size_t i=0;i<ms.getNbFace();i++)
        {
        ptr_f=ms.getFace(i);    
        if(!((ptr_f->code==code_SutureRight) || (ptr_f->code==code_doubleSutureRight)))  continue; 
        //printf("\nptr_f->label:%d -- xok:%d\n", (int)ptr_f->label, xok++);
        // we have an eligible face, does it match
        v=ptr_f->centroid-ptr_lf->centroid;v.x=0.;
        double dif = ptr_lf->centroid.y - ptr_f->centroid.y;
        auxdist=Norm(v);
        double edge1, edge2, edge3,edge4;
        
        //if((abs(dif)<1e-8))
    	//if(ptr_f->label == ((ptr_lf->label+ms.getNbFace()/2)))
        {
            int has_eq=false, trueedge=0;
      		//if(ptr_lf->nb_edge!=ptr_f->nb_edge) printf("\nIGOTTHEPOWER\n");
      		for(size_t i=0;i<ptr_lf->nb_edge;i++)
        	{ 
        		has_eq=false;
          		for(size_t j=0;(j<ptr_f->nb_edge) && (has_eq==false) ;j++)
          		{
              		if((ptr_lf->edge[i]->centroid.y==ptr_f->edge[j]->centroid.y)) 
              		{
              			vl= p[ptr_lf->edge[i]->firstVertex->label-1] - p[ptr_lf->edge[j]->firstVertex->label-1];
              			vr= p[ptr_lf->edge[i]->secondVertex->label-1] - p[ptr_lf->edge[j]->secondVertex->label-1];
              			vlx= p[ptr_lf->edge[i]->firstVertex->label-1] - p[ptr_lf->edge[j]->secondVertex->label-1];
              			vrx= p[ptr_lf->edge[i]->secondVertex->label-1] - p[ptr_lf->edge[j]->firstVertex->label-1];
              			edge1=Norm(vl); edge2=Norm(vr); edge3=Norm(vlx); edge4=Norm(vrx);
              			//printf("\nDistances: %e - %e - %e - %e\n", edge1, edge2, edge3, edge4);
              			//printf("\ncentroids::left:%f,%f,%f -- right:%f,%f,%f\n", ptr_lf->edge[i]->firstVertex->coord.x,ptr_lf->edge[i]->firstVertex->coord.y,ptr_lf->edge[i]->firstVertex->coord.z,ptr_f->edge[j]->firstVertex->coord.x,ptr_f->edge[j]->firstVertex->coord.y,ptr_f->edge[j]->firstVertex->coord.z );
              			if(((edge1<1e-8)&&(edge2<1e-8)) || ((edge3<1e-8)&&(edge4<1e-8))) {has_eq=true; printf("\nCodes: %d,%d,%d,%d\n", ptr_lf->edge[i]->firstVertex->code, ptr_lf->edge[i]->secondVertex->code, ptr_f->edge[j]->firstVertex->code, ptr_f->edge[j]->secondVertex->code );}
              			//else {printf("\nDistances: %e - %e - %e - %e\n", edge1, edge2, edge3, edge4);}
              		}
              	}
          		if((has_eq==true)) trueedge++;
        	}
        	if(trueedge==((int)ptr_lf->nb_edge))
        	{ 
        		//printf("\nOHMYGODIMAMAZNG\n");
        		//printf("\nptr_f->label:%d -- coords:%f,%f,%f -- xak:%d\n", (int)ptr_f->label, ptr_f->centroid.x, ptr_f->centroid.y, ptr_f->centroid.z, xak++);
            	dist=auxdist;
            	ptr_rf=ptr_f;
            }
            //else{ printf("\nLABEL->%d\n", (int)ptr_lf->label);}
        }
        }
    if(!ptr_rf){cout<<"Critical error, no right face"<<endl;exit(0);}
    list_face[ptr_lf->label-1]=ptr_rf;
    ptr_lf->code=CodeRemove; // code to say remove
    } 
            printf("\nEDGE\n");
ms.beginEdge();
while((ptr_le=ms.nextEdge()))
    { 
    list_edge[ptr_le->label-1]=ptr_le;
    if(!((ptr_le->code==code_SutureLeft) || (ptr_le->code==code_doubleSutureLeft))) continue;
    //if((ptr_le->code!=code_SutureLeft)) continue;
    //printf("\nptr_le->label:%d, coords:%e,%e,%e\n", (int)ptr_le->label, ptr_le->centroid.x, ptr_le->centroid.y, ptr_le->centroid.z);
    // we have a left edge, find the right edge  
    //printf("\nleft %d code:%d\n", ptr_le->label, ptr_le->code); 
    //printf("\nptr_le->label:%d, %d->coords:%e,%e,%e -- %d->%e,%e,%e\n", (int)ptr_le->label, ptr_le->firstVertex->label, ptr_le->secondVertex->label, p[ptr_le->firstVertex->label-1].x, p[ptr_le->firstVertex->label-1].y, p[ptr_le->firstVertex->label-1].z, p[ptr_le->secondVertex->label-1].x, p[ptr_le->secondVertex->label-1].y, p[ptr_le->secondVertex->label-1].z);
    ptr_re=NULL;dist=1e20;  
    for(size_t i=0;i<ms.getNbEdge();i++)
        {
        ptr_e=ms.getEdge(i);    
        if(!((ptr_e->code==code_SutureRight) || (ptr_e->code==code_doubleSutureRight)))  continue; 
        //if((ptr_e->code!=code_SutureRight) && (ptr_e->code!=code_SutureBot))  continue; 
        // we have an eligible edge, does it match
        v=ptr_e->centroid-ptr_le->centroid;v.x=0.;
        double dif = ptr_le->centroid.y - ptr_e->centroid.y;
        auxdist=Norm(v);
        bool has_eq=false;
        double edge1, edge2, edge3,edge4;
        //printf("\nptr_e->label:%d, %d->coords:%e,%e,%e -- %d->%e,%e,%e\n", (int)ptr_e->label, ptr_e->firstVertex->label, ptr_e->secondVertex->label, p[ptr_e->firstVertex->label-1].x, p[ptr_e->firstVertex->label-1].y, p[ptr_e->firstVertex->label-1].z, p[ptr_e->secondVertex->label-1].x, p[ptr_e->secondVertex->label-1].y, p[ptr_e->secondVertex->label-1].z);
        //if((ptr_le->label == 71) && (abs(dif)< 1e-8)) printf("\nabs(dif):%e\n", abs(dif));
        //if( (abs(dif)< 1e-8))
            {
            	//if((ptr_le->centroid.y==ptr_e->centroid.y)) 
              	{ 
              		vl= p[ptr_le->firstVertex->label-1] - p[ptr_e->firstVertex->label-1];
              		vr= p[ptr_le->secondVertex->label-1] - p[ptr_e->secondVertex->label-1];
              		vlx= p[ptr_le->firstVertex->label-1] - p[ptr_e->secondVertex->label-1];
              		vrx= p[ptr_le->secondVertex->label-1] - p[ptr_e->firstVertex->label-1];
              		edge1=Norm(vl); edge2=Norm(vr); edge3=Norm(vlx); edge4=Norm(vrx);
              		//if((ptr_le->label == 74) && ((edge1<1e-3) || (edge3<1e-3))) printf("\nDistances: %e - %e - %e - %e\n", edge1, edge2, edge3, edge4);
              		//if((ptr_le->label == 74) && ((edge1<1e-3) || (edge3<1e-3))) printf("\ncoordsFirst::left:%e,%e,%e -- right:%e,%e,%e\n", ptr_le->firstVertex->coord.x,ptr_le->firstVertex->coord.y,ptr_le->firstVertex->coord.z,ptr_e->firstVertex->coord.x,ptr_e->firstVertex->coord.y,ptr_e->firstVertex->coord.z );
              		//if((ptr_le->label == 74) && ((edge1<1e-3) || (edge3<1e-3))) printf("\ncoordsSecond::left:%e,%e,%e -- right:%e,%e,%e\n", ptr_le->secondVertex->coord.x,ptr_le->secondVertex->coord.y,ptr_le->secondVertex->coord.z,ptr_e->secondVertex->coord.x,ptr_e->secondVertex->coord.y,ptr_e->secondVertex->coord.z );
              		//printf("\nright %d code:%d\n", ptr_e->label, ptr_e->code);
              		if(((edge1<1e-8)&&(edge2<1e-8)) || ((edge3<1e-8)&&(edge4<1e-8))) {has_eq=true;}
              	}
            	//printf("\nptr_e->label:%d -- coords:%f,%f,%f -- xak:%d\n", (int)ptr_e->label, ptr_e->centroid.x, ptr_e->centroid.y, ptr_e->centroid.z, xak++);
            	if(has_eq==true)
            	{
            		dist=auxdist;
            		ptr_re=ptr_e;
            	}
            }
        }
    if(!ptr_re){cout<<"Critical error, no right edge, label:"<<ptr_le->label<<endl;exit(0);}
    list_edge[ptr_le->label-1]=ptr_re;
    ptr_le->code=CodeRemove; // code to say remove
    }
printf("\nVERTEX\n");
ms.beginVertex();
while((ptr_lv=ms.nextVertex()))
    {  
    list_vertex[ptr_lv->label-1]=ptr_lv;
    if(!((ptr_lv->code==code_SutureLeft) || (ptr_lv->code==code_doubleSutureLeft))) continue;
    //if((ptr_lv->code!=code_SutureLeft))  continue;
    //printf("\nptr_lv->label:%d -- coords:%e,%e,%e\n", (int)ptr_lv->label, p[ptr_lv->label-1].x, p[ptr_lv->label-1].y, p[ptr_lv->label-1].z);
    // we have a left vertex, find the right vertex
    ptr_rv=NULL;dist=1.e20;  
    for(size_t i=0;i<ms.getNbVertex();i++)
        {
        ptr_v=ms.getVertex(i);    
        if(!((ptr_v->code==code_SutureRight) || (ptr_v->code==code_doubleSutureRight)))  continue;
        //if((ptr_v->code!=code_SutureRight))  continue; 
        // we have an eligible edge, does it match
        v=p[ptr_lv->label-1] - p[ptr_v->label-1];
        //if(ptr_v->label==306) printf("\nptr_v->label:%d -- coords:%e,%e,%e\n", (int)ptr_v->label, p[ptr_v->label-1].x, p[ptr_v->label-1].y, p[ptr_v->label-1].z);
        auxdist=Norm(v);
        if(auxdist<dist )
            {
            //printf("\nptr_v->label:%d -- coords:%e,%e,%e -- xak:%d\n", (int)ptr_v->label, ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z, xak++);
            dist=auxdist;
            ptr_rv=ptr_v;
            }
        }
    if(!ptr_rv){cout<<"Critical error, no right vertex"<<endl;exit(0);}
    list_vertex[ptr_lv->label-1]=ptr_rv;
    ptr_lv->code=CodeRemove; // code to say remove
    } 
*/

    ms.beginFace();
while((ptr_lf=ms.nextFace()))
    {
    list_face[ptr_lf->label-1]=ptr_lf;
    } 
ms.beginEdge();
while((ptr_le=ms.nextEdge()))
    { 
    list_edge[ptr_le->label-1]=ptr_le;
    }
ms.beginVertex();
while((ptr_lv=ms.nextVertex()))
    {  
    list_vertex[ptr_lv->label-1]=ptr_lv;
    } 
   
// --------------- change the mesh -------------------- 
ms.beginCell();   
while((ptr_c=ms.nextCell()))
    {
    for(size_t i=0;i<ptr_c->nb_face;i++)
            ptr_c->face[i]=list_face[ptr_c->face[i]->label-1];
    }    
ms.beginFace();   
while((ptr_f=ms.nextFace()))
    {
    for(size_t i=0;i<ptr_f->nb_edge;i++) 
            ptr_f->edge[i]=list_edge[ptr_f->edge[i]->label-1];
    }    
ms.beginEdge();   
while((ptr_e=ms.nextEdge()))
    {
    ptr_e->firstVertex=list_vertex[ptr_e->firstVertex->label-1];  
    ptr_e->secondVertex=list_vertex[ptr_e->secondVertex->label-1];     
    }     
// set new labels
size_t vertex_shift=0; 
ms.beginVertex();   
//size_t code_dispLeft=para.getUnsigned("CodeDispLeft");
//size_t code_dispRight=para.getUnsigned("CodeDispRight");
while((ptr_v=ms.nextVertex()))
    {
    if(ptr_v->code==CodeRemove) 
        {
        ptr_v->label=0;    
        vertex_shift++;
        }
    else
        {
        ptr_v->label-=vertex_shift;
        //if((ptr_v->code==code_SutureLeft)/* || (ptr_v->code==code_doubleSutureLeft)*/) {printf("\nWAT\n");}
        //else if((ptr_v->code==code_SutureRight)/* || (ptr_v->code==code_doubleSutureRight)*/) {printf("\nWOT\n");}
        //else ptr_v->code=0;
        ptr_v->code=0;
        }
    }
size_t edge_shift=0; 
ms.beginEdge();   
while((ptr_e=ms.nextEdge()))
    {
    if(ptr_e->code==CodeRemove) 
        {
        ptr_e->label=0;    
        edge_shift++;
        }
    else
        {
        ptr_e->label-=edge_shift;
        //if((ptr_e->code==code_SutureLeft)/* || (ptr_e->code==code_doubleSutureLeft)*/);
        //else if((ptr_e->code==code_SutureRight)/* || (ptr_e->code==code_doubleSutureRight)*/);
        //else ptr_e->code=0;
        ptr_e->code=0;
        }
    }   
size_t face_shift=0; 
ms.beginFace();   
while((ptr_f=ms.nextFace()))
    {
    if(ptr_f->code==CodeRemove) 
        {
        ptr_f->label=0;    
        face_shift++;
        }
    else
        {
        ptr_f->label-=face_shift;
        //if((ptr_f->code==code_SutureLeft)/* || (ptr_f->code==code_doubleSutureLeft)*/);
        //else if((ptr_f->code==code_SutureRight)/* || (ptr_f->code==code_doubleSutureRight)*/);
        //else ptr_f->code=0;
        ptr_f->code=0;
        }
    } 
// write the fixation code

// ---------- write the mesh ---------
size_t nb_vertex=ms.getNbVertex()-vertex_shift;
size_t nb_edge=ms.getNbEdge()-edge_shift;
size_t nb_face=ms.getNbFace()-face_shift;
size_t nb_cell=ms.getNbCell();

string filename=para.getString("SuturedMeshFile");    
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;
	double rot = 0.0f;
	//bool finish = false;
	double Ax = 0.0f;
    double Az = para.getDouble("S");
	rot = -(para.getDouble("TransformLeft") + para.getDouble("TransformRight"))*0.5;
	rot = 0.0f;
	rot = rot * M_PI/180.0f;
for(size_t i=0;i<ms.getNbVertex();i++)
    {
    ptr_v=ms.getVertex(i);
    //rot = - para.getDouble("TransformRight") - para.getDouble("TransformLeft");
    double v_x = (p[i].x+Ax)*cos(rot) - (p[i].z+Az)*sin(rot) - Ax;
	double v_y = p[i].y;
	double v_z = (p[i].x+Ax)*sin(rot) + (p[i].z+Az)*cos(rot) - Az;
    if(ptr_v->code==CodeRemove) continue;    
    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; 
    }
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);
    if(ptr_e->code==CodeRemove) continue;     
    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);    
    if(ptr_f->code==CodeRemove) continue; 
    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();        
}




void makeDeformMesh2(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p, Parameter &para, int cicle)
{  
vector <FVVertex3D * > list_vertex(ms.getNbVertex());
vector <FVEdge3D * > list_edge(ms.getNbEdge());
vector <FVFace3D * > list_face(ms.getNbFace());
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"); 


    ms.beginFace();
while((ptr_lf=ms.nextFace()))
    {
    list_face[ptr_lf->label-1]=ptr_lf;
    } 
ms.beginEdge();
while((ptr_le=ms.nextEdge()))
    { 
    list_edge[ptr_le->label-1]=ptr_le;
    }
ms.beginVertex();
while((ptr_lv=ms.nextVertex()))
    {  
    list_vertex[ptr_lv->label-1]=ptr_lv;
    } 

size_t vertex_shift=0, edge_shift=0, face_shift=0;

/*
// --------------- change the mesh -------------------- 
ms.beginCell();   
while((ptr_c=ms.nextCell()))
    {
    for(size_t i=0;i<ptr_c->nb_face;i++)
            ptr_c->face[i]=list_face[ptr_c->face[i]->label-1];
    }    
ms.beginFace();   
while((ptr_f=ms.nextFace()))
    {
    for(size_t i=0;i<ptr_f->nb_edge;i++) 
            ptr_f->edge[i]=list_edge[ptr_f->edge[i]->label-1];
    }    
ms.beginEdge();   
while((ptr_e=ms.nextEdge()))
    {
    ptr_e->firstVertex=list_vertex[ptr_e->firstVertex->label-1];  
    ptr_e->secondVertex=list_vertex[ptr_e->secondVertex->label-1];     
    }     
// set new labels
size_t vertex_shift=0; 
ms.beginVertex();   
//size_t code_dispLeft=para.getUnsigned("CodeDispLeft");
//size_t code_dispRight=para.getUnsigned("CodeDispRight");
while((ptr_v=ms.nextVertex()))
    {
    if(ptr_v->code==CodeRemove) 
        {
        ptr_v->label=0;    
        vertex_shift++;
        }
    else
        {
        ptr_v->label-=vertex_shift;
        //if((ptr_v->code==code_SutureLeft) || (ptr_v->code==code_doubleSutureLeft)) {printf("\nWAT\n");}
        //else if((ptr_v->code==code_SutureRight) || (ptr_v->code==code_doubleSutureRight)) {printf("\nWOT\n");}
        //else ptr_v->code=0;
        ptr_v->code=0;
        }
    }
cout<<" remove "<<vertex_shift<<" vertexes"<<endl;    
size_t edge_shift=0; 
ms.beginEdge();   
while((ptr_e=ms.nextEdge()))
    {
    if(ptr_e->code==CodeRemove) 
        {
        ptr_e->label=0;    
        edge_shift++;
        }
    else
        {
        ptr_e->label-=edge_shift;
        //if((ptr_e->code==code_SutureLeft)/* || (ptr_e->code==code_doubleSutureLeft));
        //else if((ptr_e->code==code_SutureRight)/* || (ptr_e->code==code_doubleSutureRight));
        //else ptr_e->code=0;
        ptr_e->code=0;
        }
    }   
cout<<" remove "<<edge_shift<<" edges"<<endl;     
size_t face_shift=0; 
ms.beginFace();   
while((ptr_f=ms.nextFace()))
    {
    if(ptr_f->code==CodeRemove) 
        {
        ptr_f->label=0;    
        face_shift++;
        }
    else
        {
        ptr_f->label-=face_shift;
        //if((ptr_f->code==code_SutureLeft)/* || (ptr_f->code==code_doubleSutureLeft));
        //else if((ptr_f->code==code_SutureRight)/* || (ptr_f->code==code_doubleSutureRight));
        //else ptr_f->code=0;
        ptr_f->code=0;
        }
    } 
cout<<" remove "<<face_shift<<" faces"<<endl;     
*/ 
// write the fixation code

// ---------- write the mesh ---------
size_t nb_vertex=ms.getNbVertex()-vertex_shift;
size_t nb_edge=ms.getNbEdge()-edge_shift;
size_t nb_face=ms.getNbFace()-face_shift;
size_t nb_cell=ms.getNbCell();

char auxcicle[15];
sprintf(auxcicle, "%d", cicle);    
string aux1 = string(auxcicle);
string filename = string("breast_suture-" + aux1);
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;
	double rot = 0.0f;
	//bool finish = false;
	double Ax = 0.0f;
    double Az = para.getDouble("S");
	rot = -(para.getDouble("TransformLeft") + para.getDouble("TransformRight"))*0.5;
	rot = 0.0f;
	rot = rot * M_PI/180.0f;
    //printf("\nROT:%e\n", rot);
    //printf("\n\ncheckpoint A\n\n");
for(size_t i=0;i<ms.getNbVertex();i++)
    {
    ptr_v=ms.getVertex(i);
    //rot = - para.getDouble("TransformRight") - para.getDouble("TransformLeft");
    double v_x = (p[i].x+Ax)*cos(rot) - (p[i].z+Az)*sin(rot) - Ax;
	double v_y = p[i].y;
	double v_z = (p[i].x+Ax)*sin(rot) + (p[i].z+Az)*cos(rot) - Az;
    if(ptr_v->code==CodeRemove) continue;    
    //printf("\nCODE:::%d\n", ptr_v->code);
    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; 
    }
    //printf("\n\ncheckpoint B\n\n");
out_file<<"         </VERTEX>"<<endl; 
out_file<<"         <EDGE nbedge=\""<<nb_edge<<"\">"<<endl;
	//printf("\n\ncheckpoint C\n\n");
for(size_t i=0;i<ms.getNbEdge();i++)
    {
    ptr_e=ms.getEdge(i);
    if(ptr_e->code==CodeRemove) continue;     
    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;
//printf("\n\ncheckpoint D\n\n");
for(size_t i=0;i<ms.getNbFace();i++)
    {
    ptr_f=ms.getFace(i);    
    if(ptr_f->code==CodeRemove) continue; 
    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;
//printf("\n\ncheckpoint E\n\n");
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; 
    } 
   // printf("\n\ncheckpoint F\n\n");
out_file<<"         </CELL>"<<endl;      
out_file<<"    </MESH>"<<endl;
out_file<<"</FVLIB>"<<endl;          
out_file.close();        
}



void writeVolumeEnergy(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p, Parameter &para, int cicle,
						FVVect<double>  &W, FVVect<double> &WS, vector <FVDenseM<double> > &WMat)
{  
vector <FVVertex3D * > list_vertex(ms.getNbVertex());
vector <FVEdge3D * > list_edge(ms.getNbEdge());
vector <FVFace3D * > list_face(ms.getNbFace());
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;


// ---------- write the mesh ---------
size_t nb_vertex=ms.getNbVertex();
size_t nb_edge=ms.getNbEdge();
size_t nb_face=ms.getNbFace();
size_t nb_cell=ms.getNbCell();
static size_t  code_energy_surface,code_chassignac;
static bool no_cond=true;
static FVPoint3D<double> g;
static double mu,lambda,muP,lambdaP,skin,coef_chassignac,logParameter,rho;
FVPoint3D<double> OA,OB,OAp,OBp,aux;
FVPoint2D<double> C1,C2; 
double dOA,dOB,dOAp,dOBp,cosa,sina,cosap,sinap;
	code_energy_surface=para.getUnsigned("CodeSkin");
    code_chassignac=para.getUnsigned("CodeChassignac");
    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;
double tr,det,val,sum;
FVDenseM<double> DX(3),DX_bak(3);

//string filename=para.getString("SuturedMeshFile");
char auxcicle[15];
sprintf(auxcicle, "%d", cicle);    
string aux1 = string(auxcicle);
string filename = string("energy-" + aux1);
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;

//XXX2dados(ms,p,para);
for(size_t i=0;i<ms.getNbVertex();i++)
    {
    ptr_v=ms.getVertex(i);
    double auxsum = 0;
    double total_vol = 0.0f;
    ptr_v->beginCell();
	while((ptr_c=ptr_v->nextCell()))
    { 
    //cout<<"cell "<<ptr_c->code<<endl;        
    	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;
    	//cout<<"val="<<val<<" vol="<<ptr_c->volume<<endl;
    	double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    				//printf("\nVolume:%e\n", volume);
    			
    			}
    	auxsum = auxsum + val*ptr_c->volume; 
    	total_vol += ptr_c->volume;
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
    
    if(ptr_v->code==CodeRemove) continue;    
    //printf("\nCODE:::%d\n", ptr_v->code);
    //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<<scientific << setprecision(FVPRECISION) << setw(FVCHAMP) << "volume: " << total_vol << " -- energy:" << (auxsum/total_vol )<<endl; 
    }
    //printf("\n\ncheckpoint B\n\n");
out_file<<"         </VERTEX>"<<endl; 
out_file<<"         <CELL nbcell=\""<<nb_cell<<"\">"<<endl;
//printf("\n\ncheckpoint E\n\n");
for(size_t i=0;i<nb_cell;i++)
    {
    ptr_c=ms.getCell(i);         
    
    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; 
     
    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<< " volume: " << ptr_c->volume << " -- energia: " << val; 
    out_file<<endl; 
    } 
   // printf("\n\ncheckpoint F\n\n");
out_file<<"         </CELL>"<<endl;  
out_file<<"         <FACE nbface=\""<<nb_face<<"\">"<<endl;
//printf("\n\ncheckpoint D\n\n");
for(size_t i=0;i<ms.getNbFace();i++)
    {
    ptr_f=ms.getFace(i);    
    
    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]; 
        //printf("\ndet:%e -- C1:%e,%e -- C2:%e,%e -- tr:%e -- logParam:%e\n", det, C1.x, C1.y, C2.x, C2.y, tr, logParameter);
        //printf("\ndOA:%e -- dOB:%e -- dOAp:%e -- dOBp:%e\n", dOA, dOB, dOAp, dOBp);
        }
    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];
        }
    
    if(ptr_f->code==CodeRemove) continue; 
    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<< " area: " << ptr_f->area << " -- energia: " << val; 
    out_file<<endl; 
    } 
out_file<<"         </FACE>"<<endl;    
out_file<<"    </MESH>"<<endl;
out_file<<"</FVLIB>"<<endl;          
out_file.close();        
}



//  ste the code 
void setBoundaryConditions(FVMesh3D &ms,Parameter &para)
{
size_t code_fixe=para.getUnsigned("CodeFixation");
size_t code_chassignac=para.getUnsigned("CodeChassignac");
size_t code_energy_surface=para.getUnsigned("CodeSkin");
double z_min=para.getDouble("ZMinFixation");
double z_max=para.getDouble("ZMaxFixation");
double thetaFixation=para.getDouble("RotationFixation");
size_t code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
size_t code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
FVFace3D *ptr_f;
FVCell3D *ptr_c;
FVVertex3D *ptr_v;
double Az = para.getDouble("S");
double Ax = 0.0f;
double plano_rot = para.getDouble("Plano_Rot");
plano_rot = plano_rot * M_PI/180.0f;
//printf("\nplano_rot: %e\n", plano_rot);
double alphaTransf = para.getDouble("TransformLeft"), betaTransf = para.getDouble("TransformRight");
alphaTransf = alphaTransf * M_PI /180.0f;
betaTransf = betaTransf * M_PI /180.0f;
/*
	vector<int> face_codes;
	face_codes.resize(ms.getNbFace());

	for(size_t i=0;i<ms.getNbFace();i++)
	{
			int surf2 = 0, surf3 = 0, surf=0;
			ptr_f=ms.getFace(i); 
			ptr_v = ptr_f->beginVertex();
			while(ptr_v=ptr_f->nextVertex())
			{
				if(ptr_v->code == code_chassignac){ surf2++;} 
				else if(ptr_v->code == code_energy_surface){ surf3++;}
			}
			surf = surf + surf2 + surf3;
			if(surf<=2) face_codes[i] = -1;
			else if(surf2 > surf3){ face_codes[i] = code_chassignac;}
			else if(surf3 >= surf2){ face_codes[i] = code_energy_surface;}
	}
*/
ms.beginFace();   
while((ptr_f=ms.nextFace()))
    {
    if(ptr_f->rightCell) { continue;} 
    //if(ptr_f->leftCell) continue; 
    if(ptr_f->normal.y<0.6)
        {
        ptr_f->code=code_energy_surface;
        for((ptr_v=ptr_f->beginVertex());(ptr_v=ptr_f->nextVertex());)
        	ptr_v->code|=1; 
        }
    else
        {
        ptr_f->code=code_chassignac;    
        for((ptr_v=ptr_f->beginVertex());(ptr_v=ptr_f->nextVertex());)
             ptr_v->code|=2;
        }
    }     
// code vertex=1 -> vertex inside energy surface
// code vertex=2 -> vertex inside chassignac
// code vertex=3 -> on the intersection: potential fixe point
int counter = 0;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
   {
   switch(ptr_v->code)  
       {
       case 1: ptr_v->code=0;
       break;
       case 2: ptr_v->code=code_chassignac;   
       break;
       case 3: {
       FVPoint3D<double> P=ptr_v->coord;
       P.z=P.z*cos(thetaFixation)-P.y*sin(thetaFixation);
       if((P.z<=z_max) && (P.z>=z_min)) 
       {   
           ptr_v->code=code_fixe;
           counter++;
        }
       else
           ptr_v->code=code_chassignac;  
        }
       break;
       //default: 
       	//break;
       }
   }

	ms.beginFace();
	while(ptr_f=ms.nextFace())
	{
		if(	ptr_f->vertex[0]->code + ptr_f->vertex[1]->code + ptr_f->vertex[2]->code >= 4 
			&& ptr_f->code == 0 && ptr_f->vertex[0]->code != 0 && 
			ptr_f->vertex[1]->code != 0 && ptr_f->vertex[2]->code != 0) 
		{
			ptr_f->code = 2;
		}
	}


/*
vector<int> index;
ms.beginVertex();
while((ptr_v=ms.nextVertex())){
	if(ptr_v->code==code_fixe){
		if((ptr_v->coord.x>-1e-8) && (ptr_v->coord.x<1e-8)){
			index.push_back(ptr_v->label);
		}
	}
}
int tam = index.size();
printf("\nSIZE:%d\n", tam);
double menor1 = 10000, menor2 = 10000;
int ind1 = -1, ind2=-1;
for(int i=0; i<tam; i++){
	ptr_v=ms.getVertex(index[i]-1);
	if(ptr_v->coord.z < (menor1)){
		menor1 = ptr_v->coord.z;
		ind1 = index[i];
	}
	
}
for(int i=0; i<tam; i++){
	ptr_v=ms.getVertex(index[i]-1);
	if((ptr_v->coord.z < (menor2)) && (ind1!=index[i])){
		menor2 = ptr_v->coord.z;
		ind2 = index[i];
	}
	
}
for(int i=0; i<tam; i++){
	ptr_v=ms.getVertex(index[i]-1);
	if(index[i]==ind1 || index[i]==ind2);
	else{ ptr_v->code=2;}
}

vector<int> pontos_plano;
bool sai=false;
double dist=0.0f;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
{
	for(int i=0; i<numSutureP; i++){
		if(sutureP[i]==ptr_v->label){sai=true;}
	}
	if(sai) {sai=false;}
	else{
		if((plano_rot>-1e-8) && (plano_rot<1e-8)){
			if((ptr_v->coord.x>-1e-8) && (ptr_v->coord.x<1e-8) && (ptr_v->coord.z<=(Az+1e-8))){
				pontos_plano.push_back(ptr_v->label);
			}
		}
		else{
			double test = tan(M_PI*05-plano_rot)*ptr_v->coord.x + Az;	
			if((test<(ptr_v->coord.z+1e-8)) && (test>(ptr_v->coord.z-1e-8)) && (ptr_v->coord.z<=(Az+1e-8)) ){
				pontos_plano.push_back(ptr_v->label);
			}
		}
	}
	sai=false;
}

	for(int i=0; i<pontos_plano.size(); i++){
		ptr_v = ms.getVertex(pontos_plano[i]-1);
		dist = sqrt(pow(ptr_v->coord.x,2)+pow(ptr_v->coord.y,2)+pow(ptr_v->coord.z,2));
		//printf("\n Distancia do vertice %d: %e\n", (int)ptr_v->label, dist);
		ptr_v->beginCell();
		while((ptr_c=ptr_v->nextCell()))
    	{
    		ptr_c->beginFace();   
    		while((ptr_f=ptr_c->nextFace()))
        	{
				double n_length = sqrt(ptr_f->normal.x*ptr_f->normal.x + ptr_f->normal.y*ptr_f->normal.y + ptr_f->normal.z*ptr_f->normal.z);
        		if( (abs(ptr_f->normal.x) < 0.9) ) ;//printf("\nface normal: %e,%e,%e\n", ptr_f->normal.x,ptr_f->normal.y,ptr_f->normal.z );
        		else {ptr_f->code = 0; }
        		//if(ptr_f->rightCell) ptr_f->code=0;
        	}
        }	 	
	}
	
	for(size_t i=0;i<ms.getNbFace();i++)
	{
			int surf1 = 0, surf2 = 0, surf3 = 0, surf=0;
			ptr_f=ms.getFace(i); 
			ptr_v = ptr_f->beginVertex();
			while(ptr_v=ptr_f->nextVertex())
			{
				if(ptr_v->code == code_fixe){ surf1++;} 
				else if(ptr_v->code == 2){ surf2++;} 
				else if(ptr_v->code == 3){ surf3++;}
			}
			surf = surf + surf1 + surf2 + surf3;
			if(surf<=2);
			else if(surf2 > surf3){ ptr_f->code = 2; }
			else if(surf3 >= surf2){ ptr_f->code = 3; }
			if(face_codes[i] != -1) printf("\nFace_Codes %d -> %d\n", i, face_codes[i]);
	}
*/
	
	/*
	ms.beginVertex();
	while((ptr_v=ms.nextVertex()))
	{
		for(int i=0; i<numSutureP; i++){
			if(sutureP[i]==ptr_v->label){
				ptr_v->beginCell();
				while((ptr_c=ptr_v->nextCell())){   
					ptr_c->beginFace();
    				while((ptr_f=ptr_c->nextFace())){ 
    					printf("\nface normal: %e,%e,%e\n", ptr_f->normal.x,ptr_f->normal.y,ptr_f->normal.z );
    					if( (ptr_f->normal.y<0.5) && (ptr_f->normal.z<1e-8) && (abs(ptr_f->normal.x)<0.5)) ptr_f->code = 3; 
    				}	
    			}
			}
		}
	}
	*/


	
}





// initialize the vector
void initialize(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,
                vector <FVDenseM<double> > &WMat)
{
string parameter_filename="param_new.xml";
Parameter para(parameter_filename.c_str());    
//size_t code_SutureRight=para.getUnsigned("CodeRightSuturation");
FVVertex3D *ptr_v=NULL;// *ptr_mv=NULL;
//int num_vertex = ms.getNbVertex();
//int match_label;
//bool found=false;
 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); 
    }    
}


//  impose dont move condition
/*
void dontMove(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,vector<FVVertex3D *> &dirichlet)
{
FVVertex3D *ptr_v;
for(size_t i=0;i<dirichlet.size();i++)
    {
    ptr_v=dirichlet[i];    
    p[ptr_v->label-1]=ptr_v->coord;    
    }
}
*/
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,FVVect<FVPoint3D<double> > &p,Parameter &para)
{   
size_t i;  
static size_t code_fixe,code_chassignac,code_leftSuture,code_rightSuture,code_bottomSuture;
static size_t 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_bottomSuture=para.getUnsigned("CodeBottomSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide"); 
    no_cond=false;
   }
FVVertex3D *ptr_v;
NNN=0;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    { 
    	i=ptr_v->label-1; 
    	if(ptr_v->code==code_fixe) continue;
    	if(ptr_v->code==code_chassignac)
        {
         //XXX[NNN++]=p[i].y;
         XXX[NNN++]=p[i].x;
         XXX[NNN++]=p[i].z;
         continue;
        }  
    	if((ptr_v->code==code_doubleSutureLeft))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_L; ii++){
        		if(i==(leftP[ii]-1)) found = true;
        	}
        	if((left_or_right==0) || (!found)){
         		//XXX[NNN++]=p[i].y=0;
         		//XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        }       
    	if((ptr_v->code==code_doubleSutureRight))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_R; ii++){
        		if(i==(rightP[ii]-1)) found = true;
        	}
        	if((left_or_right==1) || (!found)){
         		//XXX[NNN++]=p[i].y=0;
         		//XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        } 
        if((ptr_v->code==code_leftSuture))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_L; ii++){
        		if(i==(leftP[ii]-1)) found = true;
        	}
        	if((left_or_right==0) || (!found)){
         		//XXX[NNN++]=p[i].y=0;
         		//XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].y;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        }   
    	if((ptr_v->code==code_rightSuture))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_R; ii++){
        		if(i==(rightP[ii]-1)) found = true;
        	}
        	if((left_or_right==1) || (!found)){
         		//XXX[NNN++]=p[i].y=0;
         		//XXX[NNN++]=p[i].x;
         		XXX[NNN++]=p[i].y;
         		XXX[NNN++]=p[i].z;
         	}
         continue;
        }  
    	if(ptr_v->code==code_bottomSuture)
        {
         //XXX[NNN++]=p[i].y=0;
         XXX[NNN++]=p[i].x;
         XXX[NNN++]=p[i].z;
         continue;
        }
     	XXX[NNN++]=p[i].x;XXX[NNN++]=p[i].y;XXX[NNN++]=p[i].z; 
     }
//for(i=0;i<(size_t)NNN;i++) cout<<"XXX[i]="<<XXX[i]<<endl;    
}

void XXX2dados(FVMesh3D &ms,FVVect<FVPoint3D<double> > &p,Parameter &para)
{    
size_t i;  
static size_t code_fixe,code_chassignac,code_leftSuture,code_rightSuture,code_bottomSuture;
static size_t 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_bottomSuture=para.getUnsigned("CodeBottomSuturation");
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");       
    no_cond=false;
   } 

FVVertex3D *ptr_v;//, *ptr_1, *ptr_2, *ptr_3;
NNN=0;
double Az = para.getDouble("S");
double Ax = 0.0f;
double plano_rot = para.getDouble("Plano_Rot");
plano_rot = plano_rot * M_PI/180.0f;
double alphaTransf = para.getDouble("TransformLeft"), betaTransf = para.getDouble("TransformRight");
alphaTransf = alphaTransf * M_PI /180.0f;
betaTransf = betaTransf * M_PI /180.0f;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {
    i=ptr_v->label-1;          
    //if(ptr_v->label==3 || ptr_v->label==24 || ptr_v->label==118 || ptr_v->label==4) printf("\n Vertex %d -> %d", ptr_v->label, (int)ptr_v->code);
    //if(ptr_v->label==3 || ptr_v->label==24 || ptr_v->label==118 || ptr_v->label==4) printf("\n Vertex %d -> %e,%e,%e", ptr_v->label, ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z);
    if(ptr_v->code==code_fixe) continue;
    if(ptr_v->code==code_chassignac) 
        {
        //XXX[NNN++];
        p[i].x=XXX[NNN++];
        p[i].z=XXX[NNN++];  
        continue;
        } 
    if((ptr_v->code==code_doubleSutureLeft)) 
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_L; ii++){
        		if(i==(leftP[ii]-1)) found = true;
        	}
        	if((left_or_right==0) || (!found)){
        		//p[i].y=0;p[i].x=0.;p[i].z=XXX[NNN++]; 
        		//p[i].y=0;
        		//p[i].x=XXX[NNN++];
        		//p[i].z=XXX[NNN++];
        		//if(left_or_right==0){
        		if(global_it==0){
        			p[i].y=0.0;
        			p[i].z=XXX[NNN++];
        			//p[i].z = Az - sqrt( pow(ptr_v->coord.x,2) + pow(ptr_v->coord.z - Az,2));
        			p[i].z = (ptr_v->coord.x+Ax)*sin(alphaTransf - plano_rot) + (ptr_v->coord.z+Az)*cos(alphaTransf - plano_rot) - Az;
        		}
        		else{
        			//p[i].y=0;p[i].x=0.;p[i].z=XXX[NNN++]; 
        			p[i].y=0.0;
        			//p[i].x=XXX[NNN++];
        			p[i].z=XXX[NNN++];  
        		}
        		if(plano_rot>-1e-8 && plano_rot<1e-8) p[i].x = 0.0f;
        		else {	if(plano_rot>(-1e-8)) p[i].x = (p[i].z - Az)/tan((M_PI*0.5) - plano_rot); 
        			else p[i].x = (p[i].z - Az)/tan(-(M_PI*0.5) - plano_rot);
        		}
        	}
        continue;
        }   
    if((ptr_v->code==code_doubleSutureRight)) 
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_R; ii++){
        		if(i==(rightP[ii]-1)) found = true;
        	}
        	if((left_or_right==1) || (!found)){
        		if(global_it==0){
        			p[i].y = 0.0;
        			p[i].z=XXX[NNN++];
        			//p[i].z = Az - sqrt( pow(ptr_v->coord.x,2) + pow(ptr_v->coord.z - Az,2));
        			p[i].z = (ptr_v->coord.x+Ax)*sin(-betaTransf - plano_rot) + (ptr_v->coord.z+Az)*cos(-betaTransf - plano_rot) - Az;
        		}
        		else{
        			//p[i].y=0;p[i].x=0.;p[i].z=XXX[NNN++]; 
        			p[i].y=0.0;
        			//p[i].x=XXX[NNN++];
        			p[i].z=XXX[NNN++];  
        		}
        		if(plano_rot>-1e-8 && plano_rot<1e-8) p[i].x = 0.0f;
        		else {	if(plano_rot>(-1e-8)) p[i].x = (p[i].z - Az)/tan((M_PI*0.5) - plano_rot); 
        				else p[i].x = (p[i].z - Az)/tan(-(M_PI*0.5) - plano_rot);  
        		}
        	}
        continue;
        }  
    if((ptr_v->code==code_leftSuture))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_L; ii++){
        		if(i==(leftP[ii]-1)) found = true;
        	}
        	if((left_or_right==0) || (!found)){
        		//p[i].x=0;
        		//if(left_or_right==0){
        		if(global_it==0){
        			p[i].y=XXX[NNN++];
        			p[i].z=XXX[NNN++];
        			//p[i].z = Az - sqrt( pow(ptr_v->coord.x,2) + pow(ptr_v->coord.z - Az,2));
        			p[i].z = (ptr_v->coord.x+Ax)*sin(alphaTransf - plano_rot) + (ptr_v->coord.z+Az)*cos(alphaTransf - plano_rot) - Az;
        		}
        		else{
        			p[i].y=XXX[NNN++];
        			p[i].z=XXX[NNN++]; 
        		}
        		if(plano_rot>-1e-8 && plano_rot<1e-8) p[i].x = 0.0f;
        		else {	if(plano_rot>(-1e-8)) p[i].x = (p[i].z - Az)/tan((M_PI*0.5) - plano_rot); 
        				else p[i].x = (p[i].z - Az)/tan(-(M_PI*0.5) - plano_rot);
        		}
        	}
         continue;
        }
    if((ptr_v->code==code_rightSuture))
        {
        	bool found = false;
        	for(int ii=0; ii<num_arc_R; ii++){
        		if(i==(rightP[ii]-1)) found = true;
        	}
        	if((left_or_right==1) || (!found)){
        		//p[i].x=0;
        		//if(left_or_right==0){
        		if(global_it==0){
        			p[i].y=XXX[NNN++];
        			p[i].z=XXX[NNN++];
        			//p[i].z = Az - sqrt( pow(ptr_v->coord.x,2) + pow(ptr_v->coord.z - Az,2));
        			p[i].z = (ptr_v->coord.x+Ax)*sin(-betaTransf - plano_rot) + (ptr_v->coord.z+Az)*cos(-betaTransf - plano_rot) - Az;
        		}
        		else{
        			p[i].y=XXX[NNN++];
        			p[i].z=XXX[NNN++]; 
        		}
        		if(plano_rot>-1e-8 && plano_rot<1e-8) p[i].x = 0.0f;
        		else {	if(plano_rot>(-1e-8)) p[i].x = (p[i].z - Az)/tan((M_PI*0.5) - plano_rot); 
        				else p[i].x = (p[i].z - Az)/tan(-(M_PI*0.5) - plano_rot);
        		}
        	}
        continue;            
        }          
    if(ptr_v->code==code_bottomSuture) 
        {
        p[i].y=0.0;
        p[i].x=XXX[NNN++];
        p[i].z=XXX[NNN++];  
        continue;
        }     
    p[i].x=XXX[NNN++];p[i].y=XXX[NNN++];p[i].z=XXX[NNN++];
    }
    
    //int sSut[DIM], dSut[DIM], sSutLabels[DIM*3], dSutLabels[DIM*2];
	//int num_sSuture, num_dSuture;
	//double sCoefs[DIM*3], dCoefs[DIM*2]; 
    
    //use table relation between vertex-line for doublesuture vertexes
    //Px = a1Q1x + a2Q2x;
    //Py = 0;
    //Pz = a1Q1z + a2Q2z;
    
    //use table relation between vertex-triangle for suture vertexes
    //Px = a1Q1x + a2Q2x + a3Q3x;
    //Py = a1Q1y + a2Q2y + a3Q3y;
    //Pz = a1Q1z + a2Q2z + a3Q3z;
    
    int num_vertex = ms.getNbVertex();
    if(left_or_right==0){ //transform right to left
    	
    	for(int k=0; k<num_vertex; k++)
    	{
    		ptr_v=ms.getVertex(k);
    		i=ptr_v->label-1;
    		bool found = false;
        	for(int ii=0; ii<num_arc_R; ii++){
        		if(i==(rightP[ii]-1)) found = true;
        	}
    		if((ptr_v->code==code_rightSuture) && (found))
        	{
        		int ptrlabel = ptr_v->label;
        		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;
        			}
        		}  
        	}
        	else if((ptr_v->code==code_doubleSutureRight) && (found))
        	{
        		int ptrlabel = ptr_v->label;
        		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;
        			}
        		}          
        	}  
    	}
    }
    else{ //transform left to right
    	for(int k=0; k<num_vertex; k++)
    	{
    		ptr_v=ms.getVertex(k);
    		i=ptr_v->label-1;
    		bool found = false;
        	for(int ii=0; ii<num_arc_L; ii++){
        		if(i==(leftP[ii]-1)) found = true;
        	}
    		if((ptr_v->code==code_leftSuture) && (found))
        	{
        		int ptrlabel = ptr_v->label;
        		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;
        			}
        		}           
        	}
        	else if((ptr_v->code==code_doubleSutureLeft) && (found))
        	{
        		int ptrlabel = ptr_v->label;
        		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;
        			}
        		}          
        	}  
    	}
    }
    
//for(i=0;i<(size_t)NNN;i++) cout<<"XXX[i]="<<XXX[i]<<endl;
}
// -------- evaluation of the functional -----------------
double eval(FVMesh3D &ms,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");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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];
    }
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;
        WS[ptr_f->label-1]=val*ptr_f->area;  
        sum+=WS[ptr_f->label-1];
        }
    }
return(sum);     
}
// -------- evaluation of the functional -----------------
double eval_local(FVMesh3D &ms,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_bottomSuture;
static size_t code_doubleSutureLeft;//,code_doubleSutureRight;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    //code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");  
    code_fixe=para.getUnsigned("CodeFixation");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    if(ptr_v->code==code_fixe) continue;
    if((ptr_v->code==code_doubleSutureLeft)/*||(ptr_v->code==code_doubleSutureRight)*/) {j+=2;}    
    /*if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)||
       (ptr_v->code==code_leftSuture) ||(ptr_v->code==code_rightSuture)) 
       {j+=2;}*/
       if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)) {j+=2;}
       if((ptr_v->code==code_leftSuture)) {j+=3;}
       if((ptr_v->code==code_rightSuture)) {j+=0;}
    if((ptr_v->code==0)){j+=3;}    
    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;
    double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume; 
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	
	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;
    			double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    			sum=sum - W[ptr_c->label-1] + val*ptr_c->volume; 
    			//sum=sum - W[ptr_c->label-1] + val*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;}
    }
    
}
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 
           }   
        }   
    }   
if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	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_v->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==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
	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
        	}
    	} 
    }
} 

return(sum);     
}

//eval functional of energy when transformation occurs from the right side to the left
double eval_local_right(FVMesh3D &ms,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_bottomSuture;
static size_t code_doubleSutureLeft,code_doubleSutureRight;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");  
    code_fixe=para.getUnsigned("CodeFixation");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    bool found = false;
    for(int ii=0; ii<num_arc_R; ii++){
        if((ptr_v->label-1)==(rightP[ii]-1)) found = true;
    }
    if(ptr_v->code==code_fixe) continue;
    if((ptr_v->code==code_doubleSutureLeft)) {j+=1;}    
    if((ptr_v->code==code_doubleSutureRight) && (!found)) {j+=1;}
    /*if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)||
       (ptr_v->code==code_leftSuture) ||(ptr_v->code==code_rightSuture)) 
       {j+=2;}*/
       if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)) {j+=2;}
       if((ptr_v->code==code_leftSuture)) {j+=2;}
       if((ptr_v->code==code_rightSuture) && (!found)) {j+=2;}
    if((ptr_v->code==0)){j+=3;}    
    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;
    double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume; 
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
	bool aux_found = false;
    for(int ii=0; ii<num_arc_L; ii++){
        if((ptr_v->label-1)==(leftP[ii]-1)) aux_found = true;
    }
//if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
if(aux_found){
	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;
    			double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    			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;}
    }
    
}

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 
           }   
        }   
    }   
    
//if((ptr_v->code==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
if(aux_found){
	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_v->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==code_leftSuture) || (ptr_v->code==code_doubleSutureLeft)){
if(aux_found){
	int num_s = num_sSuture;
	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
        	}
    	} 
    }
} 
return(sum);     
}



//eval functional of energy when transformation occurs from the left side to the right
double eval_local_left(FVMesh3D &ms,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_bottomSuture;
static size_t code_doubleSutureRight, code_doubleSutureLeft;
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_bottomSuture=para.getUnsigned("CodeBottomSuturation");  
    code_doubleSutureLeft=para.getUnsigned("CodeDoubleSuturationLeftSide");      
    code_doubleSutureRight=para.getUnsigned("CodeDoubleSuturationRightSide");  
    code_fixe=para.getUnsigned("CodeFixation");
    if(exec_mode==1){
    	g.x=0.0;
    	g.y=0.0;
    	g.z=0.0;    
    }
    if(exec_mode==2){
    	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;
ms.beginVertex();
while((ptr_v=ms.nextVertex()))
    {    
    bool found = false;
    for(int ii=0; ii<num_arc_L; ii++){
        if((ptr_v->label-1)==(leftP[ii]-1)) found = true;
    }
    if(ptr_v->code==code_fixe) continue;
    if((ptr_v->code==code_doubleSutureRight)/*||(ptr_v->code==code_doubleSutureRight)*/) {j+=1;}  
    if((ptr_v->code==code_doubleSutureLeft) && (!found)) {j+=1;}
    /*if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)||
       (ptr_v->code==code_leftSuture) ||(ptr_v->code==code_rightSuture)) 
       {j+=2;}*/
       if((ptr_v->code==code_chassignac)||(ptr_v->code==code_bottomSuture)) {j+=2;}
       if((ptr_v->code==code_leftSuture)&&(!found)) {j+=2;}
       if((ptr_v->code==code_rightSuture)) {j+=2;}
    if((ptr_v->code==0)){j+=3;}    
    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;
    double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    sum=sum- W[ptr_c->label-1]+ val*ptr_c->volume; 
    //sum=sum- W[ptr_c->label-1]+ val*volume;           
    }
bool aux_found = false;
    for(int ii=0; ii<num_arc_R; ii++){
        if((ptr_v->label-1)==(rightP[ii]-1)) aux_found = true;
    }
//if((ptr_v->code==code_rightSuture) || (ptr_v->code==code_doubleSutureRight)){
if(aux_found){
	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;
    			double volume = 0.0;
    			{
    				FVPoint3D<double> P1,P2;
    				FVPoint3D<double> u,v,w;
    				for(int jj=0; jj<((int)ptr_c->nb_face); jj++){
    					for(int kk=0; kk<((int)ptr_c->face[jj]->nb_edge); kk++){
    						P1 = ptr_c->face[jj]->edge[kk]->firstVertex->coord;
    						P2 = ptr_c->face[jj]->edge[kk]->secondVertex->coord;
    						u = ptr_c->cell2face[jj];
    						v = P1 - ptr_c->centroid;
    						w = P2 - ptr_c->centroid;
    						volume += fabs(Det(u,v,w))/6;
    					}
    				}
    			}
    			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;}
    }
    
}

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 
           }   
        }   
    }   
if(aux_found){
	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_v->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==code_rightSuture) || (ptr_v->code==code_doubleSutureRight)){
if(aux_found){
	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
        	}
    	} 
    }
} 
return(sum);     
}


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


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


void CalcFun_Local(FVMesh3D &ms ,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,p,para);
  //JJJ= eval_local(ms,p,W,WS,ind,initial_value,WMat,para);
  if(left_or_right==0){ 
  	//if(exec_mode==1) { 
  		XXX2dados(ms,p,para); 
  		JJJ = eval_local_right(ms,p,W,WS,ind,initial_value,WMat,para);
  	//}
  	//else { XXX2dados_disp(ms,p,para);JJJ = eval_local_dispRight(ms,p,W,WS,ind,initial_value,WMat,para); }
  	//else { XXX2dados_disp(ms,p,para);JJJ = eval_local_disp(ms,p,W,WS,ind,initial_value,WMat,para); }
  }
  else{ 
  	//if(exec_mode==1) { 
  	XXX2dados(ms,p,para); 
  	JJJ= eval_local_left(ms,p,W,WS,ind,initial_value,WMat,para);
  	//}
  	//else { XXX2dados_disp(ms,p,para); JJJ= eval_local_dispLeft(ms,p,W,WS,ind,initial_value,WMat,para);}
  	//else { XXX2dados_disp(ms,p,para); JJJ= eval_local_disp(ms,p,W,WS,ind,initial_value,WMat,para);}
  }
 
return ;
}
//---------------------------------------------------------------------------

void CalcGrad(FVMesh3D &ms ,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,p,W,WS,WMat,para);
   	JJJ0=JJJ;
   	for (int i=0; i<NNN; i++) 
    {
    	
       	YYY=XXX[i];
       	XXX[i]+=h;
       	CalcFun_Local(ms,p,W,WS,i,JJJ0,WMat,para);
       	//CalcFun(ms,p,W,WS,WMat,para);
       	RRR[i]=(JJJ-JJJ0)/h;
       	//if(RRR[i]>1e-3) cout<<"derivative "<<i/3<<"="<<RRR[i]<<endl;
       	
       	XXX[i]=YYY;
    }
    printf("\nJJJ0:%e\n", JJJ0);
	return;
}

//----------------------------------------------------------------------
void LineSearch_alt1(FVMesh3D &ms ,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,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,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,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 ,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=500;
//double c=0;
//double e=0;
//double g=0.2;
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,p,W,WS,WMat,para);
J1=JJJ;

for (int i=0; i<NNN; i++) {
  XXX[i]=XXX1[i]+(c+e)*P2[i];
}
CalcFun(ms,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=b;
alpha=a;

//if(exec_mode==1) alpha=b;
//else alpha=a;

return ;
}

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

void LineSearch(FVMesh3D &ms ,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,p,W,WS,WMat,para);
J=JJJ;
int kk=0;
for (int k=1; k<500; k++) {
//cout<<"inter k="<<k<<endl;
for (int i=0; i<NNN; i++) {
 XXX[i]=XXX1[i]+k*eps*P2[i];
}

 CalcFun(ms,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) ));

}


FVPoint3D<double> curva2plano_right(double x, double y, double z, double alphaR, double s )
{
	FVPoint3D<double> p;
	p.x = (s-z+tan(alphaR)*x) / (tan(alphaR) - tan(alphaR-(M_PI*0.5)));
	p.y = y;
	p.z = tan(alphaR-(M_PI*0.5))*p.x+s;
	return p;
}

FVPoint3D<double> curva2plano_left(double x, double y, double z, double alphaL, double s )
{
	FVPoint3D<double> p;
	p.x = (s-z-tan(alphaL)*x) / (-tan(alphaL) - tan((M_PI*0.5)-alphaL));
	p.y = y;
	p.z = z - tan(alphaL)*(p.x-x);
	return p;
}


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


//---------------------  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;
ms.read(mesh_filename.c_str());
cout<<" found "<<ms.getNbVertex()<<" vertices, "<<ms.getNbCell()<<" cells, "
               <<ms.getNbFace()<<" faces."<<endl;
// displacement vector
FVVect<FVPoint3D<double> > p(ms.getNbVertex())/*,F(ms.getNbVertex())*/, u(ms.getNbVertex());
FVVect<double> W(ms.getNbCell());
FVVect<double> WS(ms.getNbFace());
vector <FVDenseM<double> > WMat(ms.getNbCell());

exec_mode = 1; //cirurgia

	//create table
	//table_filename=para.getString("MidMeshFile");
	//FVMesh3D mTable;
	//FVVertex3D *ptr_table=NULL;
	//mTable.read(table_filename.c_str());
	//size_t code_Join=para.getUnsigned("CodeJoin"); 
	double search_radius = para.getDouble("search_radius");
	search_radius = (exp(search_radius*1.4)-1.0);
	//return 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;
	//static size_t code_doubleSutureLeft,code_doubleSutureRight;
	
    code_fixe=para.getUnsigned("CodeFixation");
    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

	FVCell3D *ptr_cell;
	ms.beginCell();
	double cut_volume = 0.0;
	while(ptr_cell = ms.nextCell())
	{
		cut_volume += ptr_cell->volume;
	}
	
	std::ofstream _of("./volume_cut.txt");
	_of << std::fixed;
	if(_of.is_open())
	{
		_of << cut_volume*1000000.0;
	}
	else
	{
		printf("\nError opening volume_cut.txt\n");
	}
	_of.close();
	
	double num_vertex = ms.getNbVertex();
	FVVertex3D *ptr_v=NULL, *ptr_aux1=NULL, *ptr_aux2=NULL, *ptr_aux3=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;
	numSutureP=0;
	for(int i=0; i<num_vertex; i++)
	{
		ptr_v = ms.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;}
		}
		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++;
		}
		if(ptr_v->code==code_matchRight){
			arc_cR++;
		}
	}
	
	//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 = arc_cR;
	//else num_sSuture = arc_cL;
	if(left_or_right==0) num_sSuture = num_r + num_dr + arc_cR;
	else num_sSuture = num_l + num_dl + arc_cL;
	//num_dSuture = num_dr;
	
	num_arc_R = arc_cR;
	num_arc_L = 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;
	double max_a = 0.0f;
	for(int i=0; i<num_vertex; i++)
	{
		ptr_v = ms.getVertex(i);
		FVPoint3D<double> p_aux;
		double auxA;
		if(ptr_v->code==code_matchLeft){
			p_aux = curva2plano_left(ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z, alphaTransf, Az);
			double auxX = p_aux.x, auxY = p_aux.y, auxZ = p_aux.z;
			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(max_a<auxA){ max_a = auxA;}
		}
		if(ptr_v->code==code_matchRight){
			p_aux = curva2plano_right(ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z, betaTransf, Az);
			double auxX = p_aux.x, auxY = p_aux.y, auxZ = p_aux.z;
			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;
			if(max_a<auxA){ max_a = 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;
		leftP[i] = which;
		angle_left[i] = mx_angle;
		ptr_v = ms.getVertex(which-1);
		//if((mx_angle>(M_PI/2.0-1e-8)) && (mx_angle<(M_PI/2.0+1e-8))) ptr_v->code = code_doubleSutureLeft;
		if((mx_angle>(max_a-1e-8)) && (mx_angle<(max_a+1e-8))) ptr_v->code = code_doubleSutureLeft;
		else {ptr_v->code = code_SutureLeft;
		sutureP[numSutureP] = ptr_v->label;
			numSutureP++;}
		//if(i==(arc_cL-1)) ptr_v->code = code_doubleSutureLeft;
		//printf("\nmx_angle:%f", mx_angle*180.0/M_PI);
	}
	//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;
		rightP[i] = which;
		angle_right[i] = mx_angle;
		ptr_v = ms.getVertex(which-1);
		//printf("\n Right arc: %d \n", ptr_v->label);
		//if((mx_angle>(M_PI/2.0-1e-8)) && (mx_angle<(M_PI/2.0+1e-8))) ptr_v->code = code_doubleSutureRight;
		if((mx_angle>(max_a-1e-8)) && (mx_angle<(max_a+1e-8))) ptr_v->code = code_doubleSutureRight;
		else{ ptr_v->code = code_SutureRight;
			sutureP[numSutureP] = ptr_v->label;
			numSutureP++;
		}
		//if(i==(arc_cL-1)) ptr_v->code = code_doubleSutureRight;
		//printf("\nmx_angle:%f", mx_angle*180.0/M_PI);
	}
	//printf("\n");
	delete [] aux_table_left;
	delete [] aux_table_right;
	delete [] aux_angle_left;
	delete [] aux_angle_right;
	
	//compute length of the arcs
	FVPoint3D<double> p_aux1, p_aux2;
	ptr_aux1 = ms.getVertex(table_left[0]-1);
	p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
	ptr_aux2 = ms.getVertex(table_right[0]-1);
	p_aux2 = curva2plano_right(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, betaTransf, Az);
	double arc_length_left=sqrt(pow(pA.x-p_aux1.x,2) + pow(pA.y-p_aux1.y,2) + pow(pA.z-p_aux1.z,2));
	double arc_length_right=sqrt(pow(pA.x-p_aux2.x,2) + pow(pA.y-p_aux2.y,2) + pow(pA.z-p_aux2.z,2));
	
	for(int i=1; i<arc_cL; i++){
		ptr_aux1 = ms.getVertex(table_left[i-1]-1);
		p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
		ptr_aux2 = ms.getVertex(table_left[i]-1);
		p_aux2 = curva2plano_left(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, alphaTransf, Az);
		arc_length_left += sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.z,2));
	}
	for(int i=1; i<arc_cR; i++){
		ptr_aux1 = ms.getVertex(table_right[i-1]-1);
		p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
		ptr_aux2 = ms.getVertex(table_right[i]-1);
		p_aux2 = curva2plano_right(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, betaTransf, Az);
		arc_length_right += sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.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;
	double ptr_aux_z, ptr_aux_x;
	FVPoint3D<double> p_aux;
	if(left_or_right==0){ //transform right to left
	
		for(int i=0; i<num_vertex; i++)
		{
			ptr_v = 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
				p_aux = curva2plano_right(ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z, betaTransf, Az);
				double vx=p_aux.x, vy=p_aux.y, vz=p_aux.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.getVertex(table_right[0]-1);
					p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
					v_point.x = pA.x + (v_angle/angle_right[0]) * (p_aux1.x-pA.x);
					v_point.y = pA.y + (v_angle/angle_right[0]) * (p_aux1.y-pA.y);
					v_point.z = pA.z + (v_angle/angle_right[0]) * (p_aux1.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.getVertex(table_right[0]-1);
					p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
					v_arc_dist += sqrt(pow(p_aux1.x-pA.x,2) + pow(p_aux1.y-pA.y,2) + pow(p_aux1.z-pA.z,2));
					bool ja_acabou = false;
					for(int tablei=1; (tablei<arc_cR) && (!ja_acabou); tablei++){
						//if(ptr_v->label==146) printf("\nangle:%f -- v_arc_dist:%e\n", angle_right[tablei]*180.0/M_PI, v_arc_dist);
						//if(ptr_v->label==146) printf("\nangles: %f -- %f\n", v_angle*180.0/M_PI, angle_right[tablei]*180.0/M_PI);
						if( v_angle > (angle_right[tablei] + 1e-8)){
							ptr_aux1 = ms.getVertex(table_right[tablei-1]-1);
							p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
							ptr_aux2 = ms.getVertex(table_right[tablei]-1);
							p_aux2 = curva2plano_right(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, betaTransf, Az);
							v_arc_dist += sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.z,2));
							//if(ptr_v->label==146) printf("\nEntrei aqui\n");
						}
						else{
							ptr_aux1 = ms.getVertex(table_right[tablei-1]-1);
							p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
							ptr_aux2 = ms.getVertex(table_right[tablei]-1);
							p_aux2 = curva2plano_right(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, betaTransf, Az);
							v_point.x = p_aux1.x + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (p_aux2.x-p_aux1.x);
							v_point.y = p_aux1.y + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (p_aux2.y-p_aux1.y);
							v_point.z = p_aux1.z + ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) * (p_aux2.z-p_aux1.z);
							v_arc_dist += sqrt(pow(v_point.x-p_aux1.x,2) + pow(v_point.y-p_aux1.y,2) + pow(v_point.z-p_aux1.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));
							//if(ptr_v->label==146) printf("\nweight: %e\n", ((v_angle-angle_right[tablei-1])/(angle_right[tablei]-angle_right[tablei-1])) );
							//if(ptr_v->label==146) printf("\nangles: %f -- %f -- %f\n", v_angle*180.0/M_PI, angle_right[tablei]*180.0/M_PI, angle_right[tablei-1]*180.0/M_PI);
						}
					}
				}
			
				tau_arc = v_arc_dist / arc_length_right;
				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.getVertex(table_left[0]-1);
				p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
				left_arc_dist = sqrt(pow(pA.x-p_aux1.x,2) + pow(pA.y-p_aux1.y,2) + pow(pA.z-p_aux1.z,2));
			
				if(new_v_arc_dist < left_arc_dist){
					ptr_aux1 = ms.getVertex(table_left[0]-1);
					p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
					v_point.x = pA.x + (new_v_arc_dist/left_arc_dist) * (p_aux1.x-pA.x);
					v_point.y = pA.y + (new_v_arc_dist/left_arc_dist) * (p_aux1.y-pA.y);
					v_point.z = pA.z + (new_v_arc_dist/left_arc_dist) * (p_aux1.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.getVertex(table_left[tablei-1]-1);
						p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
						ptr_aux2 = ms.getVertex(table_left[tablei]-1);
						p_aux2 = curva2plano_left(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, alphaTransf, Az);
						left_arc_dist = sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.z,2));
						if( new_v_arc_dist < (left_arc_dist + 1e-8)){
							v_point.x = p_aux1.x + (new_v_arc_dist/left_arc_dist) * (p_aux2.x-p_aux1.x);
							v_point.y = p_aux1.y + (new_v_arc_dist/left_arc_dist) * (p_aux2.y-p_aux1.y);
							v_point.z = p_aux1.z + (new_v_arc_dist/left_arc_dist) * (p_aux2.z-p_aux1.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;
				for(int iz=0; (iz<num_vertex) && (complete==false); iz++)
				{
					ptr_aux1 = ms.getVertex(iz);
					//colocar pontos no plano
					double ptr_aux_x1 = (Az - ptr_aux1->coord.z - tan(alphaTransf)*ptr_aux1->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
					double ptr_aux_z1 = ptr_aux1->coord.z - tan(alphaTransf)*(ptr_aux_x1-ptr_aux1->coord.x);
					//double check_test = tan(M_PI*0.5-alphaTransf)*ptr_aux_x1 + Az - ptr_aux_z1;
					size_t label1, label2, label3;
					if(((ptr_aux_x1 > (0.0-1e-8)) && (ptr_aux_x1 < (0.0+1e-8))) && ((ptr_aux_z1 > (Az-1e-8)) && (ptr_aux_z1 < (Az+1e-8))) ) in_AB = true;
					else in_AB = false;
					double v_p_dist1 = sqrt(pow(vx-ptr_aux_x1,2) + pow(vy-ptr_aux1->coord.y,2) + pow(vz-ptr_aux_z1,2));
					if(((ptr_aux1->code==code_SutureLeft) || (ptr_aux1->code==code_doubleSutureLeft) || (in_AB)) && (v_p_dist1<search_radius)){
						label1 = ptr_aux1->label;
						for(int iz2=0; (iz2<num_vertex)  && (complete==false); iz2++)
						{
							ptr_aux2 = ms.getVertex(iz2);
							double ptr_aux_x2 = (Az - ptr_aux2->coord.z - tan(alphaTransf)*ptr_aux2->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
							double ptr_aux_z2 = ptr_aux2->coord.z - tan(alphaTransf)*(ptr_aux_x2-ptr_aux2->coord.x);
							if(((ptr_aux_x2 > (0.0-1e-8)) && (ptr_aux_x2 < (0.0+1e-8))) && ((ptr_aux_z2 > (Az-1e-8)) && (ptr_aux_z2 < (Az+1e-8))) ) in_AB = true;
							else in_AB = false;
							double v_p_dist2 = sqrt(pow(vx-ptr_aux_x2,2) + pow(vy-ptr_aux2->coord.y,2) + pow(vz-ptr_aux_z2,2));
							if(((ptr_aux2->code==code_SutureLeft) || (ptr_aux2->code==code_doubleSutureLeft) || in_AB) && (ptr_aux2->label!=label1) && (v_p_dist2<search_radius)){
								label2 = ptr_aux2->label;
								for(int iz3=0; (iz3<num_vertex)  && (complete==false); iz3++)
								{
									ptr_aux3 = ms.getVertex(iz3);
									double ptr_aux_x3 = (Az - ptr_aux3->coord.z - tan(alphaTransf)*ptr_aux3->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
									double ptr_aux_z3 = ptr_aux3->coord.z - tan(alphaTransf)*(ptr_aux_x3-ptr_aux3->coord.x);
									if(((ptr_aux_x3 > (0.0-1e-8)) && (ptr_aux_x3 < (0.0+1e-8))) && ((ptr_aux_z3 > (Az-1e-8)) && (ptr_aux_z3 < (Az+1e-8))) ) in_AB = true;
									else in_AB = false;
									double v_p_dist3 = sqrt(pow(vx-ptr_aux_x3,2) + pow(vy-ptr_aux3->coord.y,2) + pow(vz-ptr_aux_z3,2));
									if(((ptr_aux3->code==code_SutureLeft) || (ptr_aux3->code==code_doubleSutureLeft) || in_AB) && (ptr_aux3->label!=label1) && (ptr_aux3->label!=label2) && (v_p_dist3<search_radius)){
										label3 = ptr_aux3->label;
										ptr_aux1 = ms.getVertex(label1-1); ptr_aux2 = ms.getVertex(label2-1); ptr_aux3 = ms.getVertex(label3-1);
										double ptr_aux_x1 = (Az - ptr_aux1->coord.z - tan(alphaTransf)*ptr_aux1->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
										double ptr_aux_z1 = ptr_aux1->coord.z - tan(alphaTransf)*(ptr_aux_x1-ptr_aux1->coord.x);
										double ptr_aux_x2 = (Az - ptr_aux2->coord.z - tan(alphaTransf)*ptr_aux2->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
										double ptr_aux_z2 = ptr_aux2->coord.z - tan(alphaTransf)*(ptr_aux_x2-ptr_aux2->coord.x);
										double ptr_aux_x3 = (Az - ptr_aux3->coord.z - tan(alphaTransf)*ptr_aux3->coord.x) / ( -tan(alphaTransf) - tan(M_PI*0.5 - alphaTransf) );
										double ptr_aux_z3 = ptr_aux3->coord.z - tan(alphaTransf)*(ptr_aux_x3-ptr_aux3->coord.x);
										
										double check0 = Az + tan(M_PI*0.5 - alphaTransf)*vx - vz;
										double check1 = Az + tan(M_PI*0.5 - alphaTransf)*ptr_aux_x1 - ptr_aux_z1;
										double check2 = Az + tan(M_PI*0.5 - alphaTransf)*ptr_aux_x2 - ptr_aux_z2;
										double check3 = Az + tan(M_PI*0.5 - alphaTransf)*ptr_aux_x3 - ptr_aux_z3;

										double at = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at1 = area_tri(vx, vy, vz, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at2 = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, vx, vy, vz, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at3 = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, vx, vy, vz );
										a1 = at1/at;
										a2 = at2/at;
										a3 = at3/at;
										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;
										}
									}
								}
							}
						}
					}
				}
				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.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
				p_aux = curva2plano_left(ptr_v->coord.x, ptr_v->coord.y, ptr_v->coord.z, alphaTransf, Az);
				double vx=p_aux.x, vy=p_aux.y, vz=p_aux.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.getVertex(table_left[0]-1);
					p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
					v_point.x = pA.x + (v_angle/angle_left[0]) * (p_aux1.x-pA.x);
					v_point.y = pA.y + (v_angle/angle_left[0]) * (p_aux1.y-pA.y);
					v_point.z = pA.z + (v_angle/angle_left[0]) * (p_aux1.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.getVertex(table_left[0]-1);
					p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
					v_arc_dist += sqrt(pow(p_aux1.x-pA.x,2) + pow(p_aux1.y-pA.y,2) + pow(p_aux1.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.getVertex(table_left[tablei-1]-1);
							p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
							ptr_aux2 = ms.getVertex(table_left[tablei]-1);
							p_aux2 = curva2plano_left(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, alphaTransf, Az);
							v_arc_dist += sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.z,2));
						}
						else{
							ptr_aux1 = ms.getVertex(table_left[tablei-1]-1);
							p_aux1 = curva2plano_left(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, alphaTransf, Az);
							ptr_aux2 = ms.getVertex(table_left[tablei]-1);
							p_aux2 = curva2plano_left(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, alphaTransf, Az);
							v_point.x = p_aux1.x + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (p_aux2.x-p_aux1.x);
							v_point.y = p_aux1.y + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (p_aux2.y-p_aux1.y);
							v_point.z = p_aux1.z + ((v_angle-angle_left[tablei-1])/(angle_left[tablei]-angle_left[tablei-1])) * (p_aux2.z-p_aux1.z);
							v_arc_dist += sqrt(pow(v_point.x-p_aux1.x,2) + pow(v_point.y-p_aux1.y,2) + pow(v_point.z-p_aux1.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;
				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.getVertex(table_right[0]-1);
				p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
				right_arc_dist = sqrt(pow(pA.x-p_aux1.x,2) + pow(pA.y-p_aux1.y,2) + pow(pA.z-p_aux1.z,2));
				//printf("\narc_length_left:%e -- new_v_arc_dist:%e\n", arc_length_left, new_v_arc_dist);
			
				if(new_v_arc_dist < right_arc_dist){
					ptr_aux1 = ms.getVertex(table_right[0]-1);
					p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
					v_point.x = pA.x + (new_v_arc_dist/right_arc_dist) * (p_aux1.x-pA.x);
					v_point.y = pA.y + (new_v_arc_dist/right_arc_dist) * (p_aux1.y-pA.y);
					v_point.z = pA.z + (new_v_arc_dist/right_arc_dist) * (p_aux1.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++){
						//printf("\nangle:%f -- v_arc_dist:%e\n", angle_right[tablei]*180.0/M_PI, v_arc_dist);
						//printf("\nangles: %f -- %f\n", v_angle*180.0/M_PI, angle_right[tablei]*180.0/M_PI);
						//printf("\nleft_arc_dist:%e -- new_v_arc_dist:%e\n", left_arc_dist, new_v_arc_dist);
						ptr_aux1 = ms.getVertex(table_right[tablei-1]-1);
						p_aux1 = curva2plano_right(ptr_aux1->coord.x, ptr_aux1->coord.y, ptr_aux1->coord.z, betaTransf, Az);
						ptr_aux2 = ms.getVertex(table_right[tablei]-1);
						p_aux2 = curva2plano_right(ptr_aux2->coord.x, ptr_aux2->coord.y, ptr_aux2->coord.z, betaTransf, Az);
						right_arc_dist = sqrt(pow(p_aux1.x - p_aux2.x,2)+pow(p_aux1.y - p_aux2.y,2)+pow(p_aux1.z - p_aux2.z,2));
						if( new_v_arc_dist < (right_arc_dist + 1e-8)){
							v_point.x = p_aux1.x + (new_v_arc_dist/right_arc_dist) * (p_aux2.x-p_aux1.x);
							v_point.y = p_aux1.y + (new_v_arc_dist/right_arc_dist) * (p_aux2.y-p_aux1.y);
							v_point.z = p_aux1.z + (new_v_arc_dist/right_arc_dist) * (p_aux2.z-p_aux1.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;
				for(int iz=0; (iz<num_vertex) && (complete==false); iz++)
				{
					ptr_aux1 = ms.getVertex(iz);
					double ptr_aux_x1 = (Az - ptr_aux1->coord.z + tan(betaTransf)*ptr_aux1->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
					double ptr_aux_z1 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x1 + Az;
					size_t label1, label2, label3;
					if(((ptr_aux_x1 > (0.0-1e-8)) && (ptr_aux_x1 < (0.0+1e-8))) && ((ptr_aux_z1 > (Az-1e-8)) && (ptr_aux_z1 < (Az+1e-8))) ) in_AB = true;
					else in_AB = false;
					double v_p_dist1 = sqrt(pow(vx-ptr_aux_x1,2) + pow(vy-ptr_aux1->coord.y,2) + pow(vz-ptr_aux_z1,2));
					if(((ptr_aux1->code==code_SutureRight) || (ptr_aux1->code==code_doubleSutureRight) || (in_AB)) && (v_p_dist1<search_radius)){
						label1 = ptr_aux1->label;
						for(int iz2=0; (iz2<num_vertex)  && (complete==false); iz2++)
						{
							ptr_aux2 = ms.getVertex(iz2);
							double ptr_aux_x2 = (Az - ptr_aux2->coord.z + tan(betaTransf)*ptr_aux2->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
							double ptr_aux_z2 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x2 + Az;
							//ptr_aux2->coord.x = ptr_aux_x;
							//ptr_aux2->coord.z = ptr_aux_z;
							//if(((ptr_aux2->coord.x > (0.0-1e-8)) && (ptr_aux2->coord.x < (0.0+1e-8))) && ((ptr_aux2->coord.z > (Az-1e-8)) && (ptr_aux2->coord.z < (Az+1e-8))) ) in_AB = true;
							//else in_AB = false;
							//double v_p_dist2 = sqrt(pow(vx-ptr_aux2->coord.x,2) + pow(vy-ptr_aux2->coord.y,2) + pow(vz-ptr_aux2->coord.z,2));
							if(((ptr_aux_x2 > (0.0-1e-8)) && (ptr_aux_x2 < (0.0+1e-8))) && ((ptr_aux_z2 > (Az-1e-8)) && (ptr_aux_z2 < (Az+1e-8))) ) in_AB = true;
							else in_AB = false;
							double v_p_dist2 = sqrt(pow(vx-ptr_aux_x2,2) + pow(vy-ptr_aux2->coord.y,2) + pow(vz-ptr_aux_z2,2));
							if(((ptr_aux2->code==code_SutureRight) || (ptr_aux2->code==code_doubleSutureRight) || in_AB) && (ptr_aux2->label!=label1) && (v_p_dist2<search_radius)){
								label2 = ptr_aux2->label;
								for(int iz3=0; (iz3<num_vertex)  && (complete==false); iz3++)
								{
									ptr_aux3 = ms.getVertex(iz3);
									double ptr_aux_x3 = (Az - ptr_aux3->coord.z + tan(betaTransf)*ptr_aux3->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
									double ptr_aux_z3 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x3 + Az;
									if(((ptr_aux_x3 > (0.0-1e-8)) && (ptr_aux_x3 < (0.0+1e-8))) && ((ptr_aux_z3 > (Az-1e-8)) && (ptr_aux_z3 < (Az+1e-8))) ) in_AB = true;
									else in_AB = false;
									double v_p_dist3 = sqrt(pow(vx-ptr_aux_x3,2) + pow(vy-ptr_aux3->coord.y,2) + pow(vz-ptr_aux_z3,2));
									if(((ptr_aux3->code==code_SutureRight) || (ptr_aux3->code==code_doubleSutureRight) || in_AB) && (ptr_aux3->label!=label1) && (ptr_aux3->label!=label2) && (v_p_dist3<search_radius)){
										label3 = ptr_aux3->label;
										ptr_aux1 = ms.getVertex(label1-1); ptr_aux2 = ms.getVertex(label2-1); ptr_aux3 = ms.getVertex(label3-1);
										double ptr_aux_x1 = (Az - ptr_aux1->coord.z + tan(betaTransf)*ptr_aux1->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
										double ptr_aux_z1 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x1 + Az;
										double ptr_aux_x2 = (Az - ptr_aux2->coord.z + tan(betaTransf)*ptr_aux2->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
										double ptr_aux_z2 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x2 + Az;
										double ptr_aux_x3 = (Az - ptr_aux3->coord.z + tan(betaTransf)*ptr_aux3->coord.x) / ( tan(betaTransf) - tan(-M_PI*0.5 + betaTransf) );
										double ptr_aux_z3 = tan(-M_PI*0.5+betaTransf)*ptr_aux_x3 + Az;
										double check0 = Az + tan(-M_PI*0.5 + betaTransf)*vx - vz;
										double check1 = Az + tan(-M_PI*0.5 + betaTransf)*ptr_aux_x1 - ptr_aux_z1;
										double check2 = Az + tan(-M_PI*0.5 + betaTransf)*ptr_aux_x2 - ptr_aux_z2;
										double check3 = Az + tan(-M_PI*0.5 + betaTransf)*ptr_aux_x3 - ptr_aux_z3;

										double at = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at1 = area_tri(vx, vy, vz, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at2 = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, vx, vy, vz, ptr_aux_x3, ptr_aux3->coord.y, ptr_aux_z3 );
										double at3 = area_tri(ptr_aux_x1, ptr_aux1->coord.y, ptr_aux_z1, ptr_aux_x2, ptr_aux2->coord.y, ptr_aux_z2, vx, vy, vz );
										a1 = at1/at;
										a2 = at2/at;
										a3 = at3/at;
										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;
										}
									}
								}
							}
						}
					}
				}
				if(complete==false) {printf("\nN\ao encontrou match\n\n\n\n\n\n"); fail_counter++;}
			}
		}
	}
	//double t_x, t_y, t_z;


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


global_it = 0;
initialize(ms,p,WMat);
cout<<"initialisation done"<<endl;fflush(NULL);

static double NormEpsilon=0.,MaxIter=0.;
static bool no_cond=true;
if(no_cond)
    { 
    NormEpsilon=para.getDouble("NormEpsilon");
    MaxIter=para.getDouble("MaxIter");
    no_cond=false;
    }


dados2XXX(ms,p,para);
CalcGrad(ms,p,W,WS,WMat,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,p,W,WS,WMat,para);
LineSearch_alt1(ms,p,W,WS,WMat,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];
}
size_t count=0,nb_iter=0;
double deslocX=0, deslocY=0, deslocZ=0;
//while ( (nor1 > NormEpsilon) && (nb_iter< MaxIter) ) 
double act_energy = 10000000000.0f;
double energy_variation = 1e-5;
double global_energy = JJJ;
while( (fabs(act_energy - global_energy) > energy_variation) && (nb_iter<1000))
{  //while
	deslocX = p[94].x, deslocY = p[94].y, deslocZ = p[94].z;
    nb_iter++;
    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,p,W,WS,WMat,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;
    count++;
    if ((int)count>NNN) 
        {
        count=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,p,W,WS,WMat,para);
    LineSearch_alt1(ms, p, W, WS, WMat, 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];
        }
        //for(int j=0; j<num_vertex; j++){
        //}
        
        global_energy = JJJ;
    //if(count%10 == 0)
    {
    	string cfilename = string("percentage.txt");
		ofstream  counter_file;    
		counter_file.open(cfilename.c_str());
		double percentage = ((double)nb_iter)/((double)MaxIter) * 100;
		printf("\nPercentage: %f\n", percentage);
        counter_file << percentage <<endl;
        counter_file.close(); 
        //FVVertex3D *ptr_v;
		//NNN=0;
    }
    
    if(count%100 == 0){
    
    FVCell3D *ptr_c;
    FVVertex3D *n1, *n2, *n3, *n0;
    FVPoint3D<double> u1, u2, u3, v1, v2, v3;
      size_t nb_cell=ms.getNbCell();
      for(size_t i=0;i<nb_cell;i++)
   	 	{
   	 		ptr_c = ms.getCell(i);   
   	 		n0 = ptr_c->vertex[0]; n1 = ptr_c->vertex[1];
   	 		n2 = ptr_c->vertex[2]; n3 = ptr_c->vertex[3];
   	 		u1 = n1->coord - n0->coord;
   	 		u2 = n2->coord - n0->coord;
   	 		u3 = n3->coord - n0->coord;
   	 		v1 = p[n1->label-1] - p[n0->label-1];
   	 		v2 = p[n2->label-1] - p[n0->label-1];
   	 		v3 = p[n3->label-1] - p[n0->label-1];
   			double det1 = Det(u1,u2,u3);
   			double det2 = Det(v1,v2,v3);
    		//if((det1<0) && (det2>0)) printf("\ncell %d with det %e\n", ptr_c->label, det1, det2);
    		//else if((det1>0) && (det2<0)) printf("\ncell %d with det %e\n", ptr_c->label, det1, det2);
    	}
    }
    global_it = count;
    //printf("iteration %lu  with residual %8.5e <-> energy_variation:%e                      \r",count,nor1,fabs(act_energy - global_energy));    fflush(NULL);
    }//end of while
printf("\n \n final iteration %lu  with residual %8.5e <-> energy_variation:%e                       \r",count,nor1, fabs(act_energy - global_energy));    fflush(NULL);  
      
XXX2dados(ms,p,para); // return to the C++ world
//
//
//
//save the final  position
	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();
	
string displacement_filename,position_filename;
position_filename=para.getString("CirurgiaPositionFile");
FVio position_file(position_filename.c_str(),FVWRITE);
position_file.put(p,0.0,"position"); 
// compute the displacement
{
FVVertex3D *ptr_v;
//size_t    code_fixe=para.getUnsigned("CodeFixation");
//size_t    code_bottomSuture=para.getUnsigned("CodeBottomSuturation");
ms.beginVertex();
while ((ptr_v=ms.nextVertex()))
    {
    size_t i=ptr_v->label-1;    
    u[i]=p[i]-ptr_v->coord;
    //if(ptr_v->code==code_fixe) 
      //  {cout<<"find a fixation code"<<endl; ptr_v->code=code_bottomSuture;}
    }
}
//save the final displacement
displacement_filename=para.getString("CirurgiaDisplacementFile");
FVio displacement_file(displacement_filename.c_str(),FVWRITE);
displacement_file.put(u,0.0,"displacement"); 

// save the new mesh
makeDeformMesh(ms,p,para);
mesh_filename=para.getString("SuturedMeshFile");

FVMesh3D mm;
mm.read(mesh_filename.c_str());
setBoundaryConditions(mm,para);
mm.write(mesh_filename.c_str());

return 0;
/*
//----------//
//DISPLACEMENT
exec_mode = 2; //displacement

printf("\nDISPLACEMENT\n");
mesh_filename=para.getString("SuturedMeshFile");
// read the mesh file
FVMesh3D ms2;
ms2.read(mesh_filename.c_str());fflush(NULL);
cout<<" found "<<ms2.getNbVertex()<<" vertices, "<<ms2.getNbCell()<<" cells, "
               <<ms2.getNbFace()<<" faces."<<endl;
// displacement vector
FVVect<FVPoint3D<double> > p2(ms2.getNbVertex()),F2(ms2.getNbVertex()), u2(ms2.getNbVertex());
FVVect<double> W2(ms2.getNbCell());
FVVect<double> WS2(ms2.getNbFace());
vector <FVDenseM<double> > WMat2(ms2.getNbCell());
initialize(ms2,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;
    }

printf("\n1\n");
dados2XXX_disp(ms2,p2,para);
printf("\n2\n");
CalcGrad(ms2,p2,W2,WS2,WMat2,para);
printf("\n3\n");
for (int i=0; i<NNN; i++) {
 P2[i]=-RRR[i]; 
}

for (int i=0; i<NNN; i++) {
 XXX1[i]=XXX[i];
}
printf("\n4\n");
LineSearch1(ms2,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;
int print_ind = 50;
deslocX=0; deslocY=0; deslocZ=0;
printf("\n5\n");
//while ( (nor1 > NormEpsilon2) && (nb_iter2< MaxIter2) ) 
while ( (nor1 > NormEpsilon2) && (nb_iter2< 10) ) 
{  //while
	deslocX = p2[print_ind].x, deslocY = p2[print_ind].y, deslocZ = p2[print_ind].z;
    nb_iter2++;
    for (int i=0; i<NNN; i++) 
        {
        XXX[i]=XXX1[i];
        }
    for (int i=0; i<NNN; i++) 
        {
        RRR1[i]=RRR[i];
        }
    CalcGrad(ms2,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(ms2,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];
        }
        
        printf("\nVertex 2 -> %e,%e,%e", p2[print_ind].x, p2[print_ind].y, p2[print_ind].z);
        printf("\ndesloc 2 -> %e,%e,%e", p2[print_ind].x - deslocX, p2[print_ind].y-deslocY, p2[print_ind].z-deslocZ);
        
    printf("iteration %lu  with residual %8.5e                       \r",count2,nor1);    fflush(NULL);
    //cout<<"iteration "<<count<<" with residual "<<nor1<<endl;
    }//end of while
printf("\n \n final iteration %lu  with residual %8.5e                       \r",count2,nor1);    fflush(NULL);  
//cout<<"final step"<<count<<" with residual "<<nor1<<endl; fflush(NULL);
XXX2dados_disp(ms2,p2,para); // return to the C++ world
//
//
//
//save the final  position
string displacement_filename2,position_filename2;
position_filename2=para.getString("SuturedPositionFile");
FVio position_file2(position_filename2.c_str(),FVWRITE);
position_file2.put(p2,0.0,"position"); 
// compute the displacement
{
FVVertex3D *ptr_v2;
ms2.beginVertex();
while ((ptr_v2=ms2.nextVertex()))
    {
    size_t i=ptr_v2->label-1;    
    u2[i]=p2[i]-ptr_v2->coord;
    }
}
//save the final displacement
displacement_filename2=para.getString("SuturedDisplacementFile");
FVio displacement_file2(displacement_filename2.c_str(),FVWRITE);
displacement_file2.put(u2,0.0,"displacement");
         */      
               

}