/////////////////////////////////////////////////////
//  Homogenization Method for the minimization     //
//        of a cantilever's compliance             //
//                                                 //
//            CMAP, Ecole Polytechnique            //
//      Copyright G. Allaire, O. Pantz 2005        //
/////////////////////////////////////////////////////

ofstream fichobj("cantilever.obj");
string sauve="cantilever";	// File name where the data are saved
int niternp=60;			// Number of non penalized iterations
int niterpp=70;			// Number of penalized iterations
int niter=niternp+niterpp;	// Total number of iterations
int p=0;			// Degree of penalization:  p big-> smaller penalization
int n=1;			// Scale the size of the mesh
real compliance;		// Compliance
string legende;			// Caption of the graphical outputs
real Y=15;			// Young Modulus
real nu=0.35;			// Poisson Ratio
real lagrange=1;		// Lagrange Multiplier for the volume constraint
real lagmin, lagmax;		// Minimum and maximum value of the Lagrange Multiplier
real masse0;			// Initial volume
real masse,masse1;		// Current volume
int inddico ;			// Index for the search of the Lagrange Multiplier
real epsil=0.01;		// Ratio between the rigidity of the soft and real material
real mascible=95.;
real thetamoy;			// Average density
real pi=4*atan(1);		// Value of pi=3.14159...
real sqrt2 = sqrt(2.);		// square root of 2
func f1=0;  			// Applied loads
func f2=-1;

// Definition of the isovalues of the graphical outputs
real[int] vviso(20);
for (int i=0;i<20;i++)
vviso[i]=i*0.05+0.05 ;
/////////////////////////////////////
// Computation of the Lame moduli  //
/////////////////////////////////////
real lambda=Y*nu/((1.+nu)*(1.-2.*nu));
real mu=Y/(2.*(1.+nu));
real K=(mu+lambda)/(mu*(2.*mu+lambda));
real kappa=lambda+mu;

mesh Th;
border bd1(t=-8,-1)	{ x=20; y=t;label=2; };   //right boundary of the shape
border bd2(t=-1,1)	{ x=20;  y=t;label=3; };
border bd3(t=1,8)	{ x=20;  y=t;label=2; };
border bg1(t=8,4)	{ x=0; y=t;label=2; };
border bg2(t=4,-4)	{ x=0; y=t;label=1; };    //left boundary of the shape
border bg3(t=-4,-8)	{ x=0; y=t;label=2; };
border bs(t=20,0)	{ x=t;  y=8;label=2; };   //upper boundary of the shape
border bi(t=0,20)	{ x=t;  y=-8;label=2; };  //lower boundary of the shape

//////////////////////////////
// Definition of the mesh   //
//////////////////////////////
int m=int(4.5*n) ;
Th= buildmesh (bg1(3*n)+bg2(10*n)+bg3(3*n)+bs(20*n)+bi(20*n)+bd1(m)+bd2(n)+bd3(m));

real mastot=int2d(Th)(1.);
thetamoy=mascible/mastot;
savemesh(Th,sauve+".msh");

//////////////////////////////////
// Finite elements spaces       //
// (u,v) = displacement         //
// sigma = stress tensor        //
// theta = density of material  //
//////////////////////////////////
fespace Vh1(Th,P1);
fespace Vh2(Th,[P2,P2]);

Vh2 [u,v],[w,s];
Vh1 theta,theta1,A,B,C,D,E,F,m1,m2,A1,B1,C1,D1,E1,F1,DET;
Vh1 sigma11,sigma22,sigma12,vp1,vp2,e1x,e1y,e2x,e2y;

////////////////////
// Initialization //
////////////////////
func theta0=thetamoy;
theta=theta0;
//initial volume
real volume=int2d(Th)(1.);
masse0=int2d(Th)(theta);
masse0 = masse0/volume ;

///////////////////////////////////////////////////////////////////////////
// Tensorial Notations:                                                  //
// The symmetric 2x2 matrices are arranged in a vector with 3 components //
// M=(M11,M22,sqrt(2)*M12)^t so the norm of the vector                   //
// is equal to the norm of the matrix.                                   //
// A 4th order symmetric tensor is then a 3x3 symmetric matrix.          //
// The elasticity tensor is given by:                                    //
//	(	A	D	F	)				 //
//	(	D	B	E	)				 //
//	(	F	E	C	)				 //
// See below for the computation of sigma in terms of e(u).           //
///////////////////////////////////////////////////////////////////////////
//initial tensor (domain full of material)
A = lambda+2*mu;
B = lambda+2*mu;
C = 2*mu;
D = lambda;
E = 0;
F = 0;
//initial tensor (upper Hashin and Shtrikman bound)
real kapef, muef;
kapef = thetamoy*kappa*mu/((1.-thetamoy)*kappa+mu) ;
muef = thetamoy*kappa*mu/(kappa+(1.-thetamoy)*(kappa+2*mu)) ;
C = 2.*muef ;
D = kapef - muef ;
A = D+C;
B = D+C;
E = 0;
F = 0;
//////////////////////////////
// Variational formulation  //
//////////////////////////////
problem elasticite([u,v],[w,s]) =
int2d(Th)(	 (A*dx(u)+D*dy(v)+F*sqrt2*(dx(v)+dy(u))/2) 	* dx(w)
		+(D*dx(u)+B*dy(v)+E*sqrt2*(dx(v)+dy(u))/2) 	* dy(s)
		+(F*dx(u)+E*dy(v)+C*sqrt2*(dx(v)+dy(u))/2) 	* sqrt2*(dx(s)+dy(w))/2	)
- int1d(Th,3)( f1*w + f2*s )
+ on(1,u=0,v=0)
;

//////////////////////////////////
//     Optimization Loop        //
//////////////////////////////////

int iter;
for (iter=1;iter< niter+1;iter=iter+1)
{
cout <<"Iteration " <<iter <<" ----------------------------------------" <<endl;

elasticite;

compliance = int1d(Th,3)(f1*u+f2*v);

cout<<"compliance = "<<compliance<<endl;
fichobj<<compliance<<endl;

/////////////////////////
// Stress tensor sigma //
/////////////////////////
sigma11 = A*dx(u)+	D*dy(v)+	F*sqrt2*(dx(v)+dy(u))/2;
sigma22 = D*dx(u)+	B*dy(v)+	E*sqrt2*(dx(v)+dy(u))/2;
sigma12 =(F*dx(u)+	E*dy(v)+	C*sqrt2*(dx(v)+dy(u))/2)/sqrt2;

/////////////////////////////////////
// Diagonalization of sigma        //
//  vp1, vp2 = eigenvalues         //
// (e1x,e1y) = first eigen vector  //
// (e2x,e2y) = second eigen vector //
/////////////////////////////////////
vp1 = (sigma11+sigma22+sqrt((sigma11-sigma22)^2+4*sigma12^2))/2;
vp2 = (sigma11+sigma22-sqrt((sigma11-sigma22)^2+4*sigma12^2))/2;

real delta = epsil*1.e-6 ;
e1x = sigma12/sqrt(sigma12^2+(vp1-sigma11)^2+delta^2);
e1y = (vp1-sigma11)/sqrt(sigma12^2+(vp1-sigma11)^2+delta^2);
e2x = e1y ;
e2y = -e1x ;

/////////////////////////////////////
// Proportions of rank 2 laminate  //
/////////////////////////////////////
m1 = abs(vp2)/(abs(vp1)+abs(vp2));
m2 = abs(vp1)/(abs(vp1)+abs(vp2));

//////////////////////////////////////////////////
// Computation of the density theta of material //
//////////////////////////////////////////////////
theta1 = sqrt(mu+kappa)/sqrt((4*mu*kappa))*(abs(vp1)+abs(vp2));
theta = theta1/sqrt(lagrange);
theta = min(1,theta);

/////////////////////////////////////////////////
// Computation of the Lagrange Multiplier      //
/////////////////////////////////////////////////
masse = int2d(Th)(theta)/volume;
masse1 = masse;

// Bounds for the Lagrange multiplier
if (masse1 < masse0)
{
   lagmin = lagrange ;
   while (masse < masse0)
{
      lagrange = lagrange/2 ;
      theta = theta1/sqrt(lagrange);
      theta = min(1,theta);
      masse = int2d(Th)(theta)/volume ;
};
   lagmax = lagrange ;
};
//
if (masse1 > masse0)
{
   lagmax = lagrange ;
   while (masse > masse0)
{
      lagrange = 2*lagrange ;
      theta = theta1/sqrt(lagrange);
      theta = min(1,theta);
      masse = int2d(Th)(theta)/volume ;
};
   lagmin = lagrange ;
};
//
if (masse1 == masse0)
{
   lagmin = lagrange ;
   lagmax = lagrange ;
};

// Dichotomy on the Lagrange multiplier
inddico=0;
while ((abs(1.-masse/masse0)) > 0.000001)
{
   lagrange = (lagmax+lagmin)*0.5 ;
      theta = theta1/sqrt(lagrange);
      theta = min(1,theta);
   masse = int2d(Th)(theta)/volume ;
   inddico=inddico+1;
   if (masse < masse0)
      {lagmin = lagrange ;} ;
   if (masse > masse0)
      {lagmax = lagrange ;} ;

};

cout<<"Number of Lagrange's Iterations "<<inddico<<endl;

////////////////////////////
// Penalization of theta  //
////////////////////////////
if (iter>niternp)
{theta=((1-cos(pi*theta))/2+p*theta)/(p+1);}
;

///////////////////////////////////////////
// Computation of the homogenized tensor //
///////////////////////////////////////////

// Contribution of the directions of lamination
A =	m1 * ( (lambda+2*mu) - 1/mu*(lambda^2*e1y^2+(lambda+2*mu)^2*e1x^2) + K*((lambda+2*mu)*e1x^2+lambda*e1y^2)^2 )
	+m2* ( (lambda+2*mu) - 1/mu*(lambda^2*e2y^2+(lambda+2*mu)^2*e2x^2) + K*((lambda+2*mu)*e2x^2+lambda*e2y^2)^2 );
B =	m1 * ( (lambda+2*mu) - 1/mu*(lambda^2*e1x^2+(lambda+2*mu)^2*e1y^2) + K*((lambda+2*mu)*e1y^2+lambda*e1x^2)^2 )
	+m2* ( (lambda+2*mu) - 1/mu*(lambda^2*e2x^2+(lambda+2*mu)^2*e2y^2) + K*((lambda+2*mu)*e2y^2+lambda*e2x^2)^2 );
C =	m1 * ( 4*mu - 1/mu*(2*mu)^2 + K*(4*mu*e1x*e1y)^2 )/2
	+m2* ( 4*mu - 1/mu*(2*mu)^2 + K*(4*mu*e2x*e2y)^2 )/2;
D = 	m1 * ( 2*lambda - 1/mu*(2*lambda*(lambda+2*mu)) + K*2*((lambda+2*mu)*e1y^2+lambda*e1x^2)*((lambda+2*mu)*e1x^2+lambda*e1y^2) )/2
	+m2* ( 2*lambda - 1/mu*(2*lambda*(lambda+2*mu)) + K*2*((lambda+2*mu)*e2y^2+lambda*e2x^2)*((lambda+2*mu)*e2x^2+lambda*e2y^2) )/2;
E =	m1 * ( -1/mu*(4*mu*(2*lambda+2*mu)*e1x*e1y) + K*2*(4*mu*e1x*e1y*((lambda+2*mu)*e1y^2+lambda*e1x^2)) )/(2*sqrt2)
	+m2* ( -1/mu*(4*mu*(2*lambda+2*mu)*e2x*e2y) + K*2*(4*mu*e2x*e2y*((lambda+2*mu)*e2y^2+lambda*e2x^2)) )/(2*sqrt2);
F = 	m1 * ( -1/mu*(4*mu*(2*lambda+2*mu)*e1x*e1y) + K*2*(4*mu*e1x*e1y*((lambda+2*mu)*e1x^2+lambda*e1y^2)) )/(2*sqrt2)
	+m2* ( -1/mu*(4*mu*(2*lambda+2*mu)*e2x*e2y) + K*2*(4*mu*e2x*e2y*((lambda+2*mu)*e2x^2+lambda*e2y^2)) )/(2*sqrt2);

// Addition of a soft material (almost void)
A = epsil/(1.-epsil)*(lambda+2*mu)	+ theta*A;
B = epsil/(1.-epsil)*(lambda+2*mu)	+ theta*B;
C = epsil/(1.-epsil)*2*mu		+ theta*C;
D = epsil/(1.-epsil)*lambda		+ theta*D;
E = epsil/(1.-epsil)*0			+ theta*E;
F = epsil/(1.-epsil)*0			+ theta*F;

//First inversion
DET = A*B*C-A*E*E-B*F*F-C*D*D+2*D*E*F;
A1=(B*C-E*E)/DET;
B1=(A*C-F*F)/DET;
C1=(A*B-D*D)/DET;
D1=(E*F-C*D)/DET;
E1=(D*F-A*E)/DET;
F1=(D*E-B*F)/DET;

A = (1-theta)*A1+(lambda+2*mu)/(4*mu*(lambda+mu));
B = (1-theta)*B1+(lambda+2*mu)/(4*mu*(lambda+mu));
C = (1-theta)*C1+1/(2*mu);
D = (1-theta)*D1-lambda/(4*mu*(lambda+mu));
E = (1-theta)*E1;
F = (1-theta)*F1;

//Second inversion
DET = A*B*C-A*E*E-B*F*F-C*D*D+2*D*E*F;
A1=(B*C-E*E)/DET;
B1=(A*C-F*F)/DET;
C1=(A*B-D*D)/DET;
D1=(E*F-C*D)/DET;
E1=(D*F-A*E)/DET;
F1=(D*E-B*F)/DET;
A=A1;
B=B1;
C=C1;
D=D1;
E=E1;
F=F1;

legende="Iteration "+iter+", Compliance "+compliance+", Volume="+masse*mastot;
plot(theta,fill=1,viso=vviso,cmm=legende);

if (iter == niternp)
{
plot(theta,fill=1,viso=vviso,cmm=legende,ps=sauve+"-homog-legende.eps");
plot(theta,fill=1,viso=vviso,ps=sauve+"-homog.eps");
}
;
//Mesh adaptation
if(iter<=niternp)
{
Th=adaptmesh(Th,[u+theta*0.05,v+theta*0.05],err=0.001);
}
else
{Th=adaptmesh(Th,[u,v],err=0.001);};
};



legende="Final Shape, Iteration "+niter+", Compliance "+compliance+", Volume="+masse*mastot;
plot(theta,fill=1,viso=vviso,cmm=legende,ps=sauve+"-legende.eps");
plot(theta,fill=1,viso=vviso,ps=sauve+".eps");