// this exemple is buggus in some case ...
// strange ...  FH.  , S Auliac  (will be correct in test ???)
// remove the test  (no loop)  
int nadapt=0;//  3 is teh previous value..
load "medit";
load "ff-Ipopt";

real alpha=0.9;
int np=30;
mesh Th = square(2*np,np,[2*pi*x,pi*y]);

// FFCS regression reference value
real regtest;

fespace Vh(Th,P1,periodic=[[2,y],[4,y]/*,[1,x],[3,x]*/]);

/*
 * Initial shape definition
 * outside of the mesh adaptation loop to initialize with the previous optimial shape found on further iterations
 */

Vh startshape=5;//+0.5*(cos(10*x)*sin(10*y)*sin(y));

//uz,lz,lm : dual variables storage for the warmstart
Vh uz=1.,lz=1.;
real[int] lm=[1];



for(int kkk=0;kkk<nadapt;++kkk) //Mesh adaptation loop
{

int iter=0;
func sin2 = square(sin(y));

/*
 * A function which transform Th in 3d mesh (r=rho)
 * a point (theta,phi) of Th becomes ( r(theta,phi)*cos(theta)*sin(phi) , r(theta,phi)*sin(theta)*sin(phi) , r(theta,phi)*cos(phi) )
 * then displays the resulting mesh with medit.
 * (often crashs if some values of rho are negative or if freefem is compiled with debug)
 */
func int Plot3D(real[int] &rho,string cmm,bool ffplot)
{
	Vh rhoo;
	rhoo[]=rho;
	//mesh sTh = square(np,np/2,[2*pi*x,pi*y]);
	//fespace sVh(sTh,P1);
	//Vh rhoplot=rhoo;
	try
	{
		meshS Sphere = movemesh23(Th,transfo=[rhoo(x,y)*cos(x)*sin(y),rhoo(x,y)*sin(x)*sin(y),rhoo(x,y)*cos(y)]);
		if(ffplot) plot(Sphere);
		else medit(cmm ,Sphere,wait=0);
	}
	catch(...)
	{
		cout << "cant plot" << endl;
	}
	return 1;
}


/*
 * Surface computation
 * Maybe is it possible to use movemesh23 to have the surface function less complicated
 * However, it would not simplify the gradient and the hessian
 */
func real Area(real[int] &X)
{
	Vh rho;
	rho[] = X;
	Vh rho2 = square(rho);
	Vh rho4 = square(rho2);
	real res = int2d(Th)(sqrt( rho4*sin2 + rho2*square(dx(rho)) + rho2*sin2*square(dy(rho)) ) );
	++iter;
	if(1) plot(rho,value=1,fill=1,cmm="rho(theta,phi) on [0,2pi]x[0,pi] - S="+res,dim=3);
	else Plot3D(rho[],"shape_evolution",1);
	return res;
}
func real[int] GradArea(real[int] &X)
{
	Vh rho,rho2;
	rho[] = X;
	rho2[] = square(X);
	Vh sqrtPsi,alpha;
	{
		Vh  dxrho2 = dx(rho)*dx(rho), dyrho2 = dy(rho)*dy(rho);
		sqrtPsi = sqrt( rho2*rho2*sin2 + rho2*dxrho2 + rho2*dyrho2*sin2 );
		alpha = 2.*rho2*rho*sin2 + rho*dxrho2 + rho*dyrho2*sin2;
	}
	varf dArea(u,v) = int2d(Th)(1./sqrtPsi * (alpha*v + rho2*dx(rho)*dx(v) + rho2*dy(rho)*sin2*dy(v)) );
	real[int] grad = dArea(0,Vh);
	return grad;
}
matrix hessianA;
func matrix HessianArea(real[int] &X)
{
	Vh rho,rho2;
	rho[] = X;
	rho2 = square(rho);
	Vh sqrtPsi,sqrtPsi3,C00,C01,C02,C11,C12,C22,A;
	{
		Vh C0,C1,C2;
		Vh  dxrho2 = dx(rho)*dx(rho), dyrho2 = dy(rho)*dy(rho);
		sqrtPsi = sqrt( rho2*rho2*sin2 + rho2*dxrho2 + rho2*dyrho2*sin2 );
		sqrtPsi3 = ( rho2*rho2*sin2 + rho2*dxrho2 + rho2*dyrho2*sin2 )*sqrtPsi;
		C0 = 2*rho2*rho*sin2 + rho*dxrho2 + rho*dyrho2*sin2;
		C1 = rho2*dx(rho);
		C2 = rho2*sin2*dy(rho);
		C00 = square(C0);
		C01 = C0*C1;
		C02 = C0*C2;
		C11 = square(C1);
		C12 = C1*C2;
		C22 = square(C2);
		A = 6.*rho2*sin2 + dxrho2 + dyrho2*sin2;
	}
	varf d2Area(w,v) =
		int2d(Th)(1./sqrtPsi * (A*w*v + 2*rho*dx(rho)*dx(w)*v + 2*rho*dx(rho)*w*dx(v) + 2*rho*dy(rho)*sin2*dy(w)*v + 2*rho*dy(rho)*sin2*w*dy(v)
								+ rho2*dx(w)*dx(v) + rho2*sin2*dy(w)*dy(v)) 
		+        1./sqrtPsi3 * (C00*w*v + C01*dx(w)*v + C01*w*dx(v) + C02*dy(w)*v + C02*w*dy(v) + C11*dx(w)*dx(v)
								   + C12*dx(w)*dy(v) + C12*dy(w)*dx(v) + C22*dy(w)*dy(v)) );
	hessianA = d2Area(Vh,Vh);
	return hessianA;
}


/*
 * Volume computation
 */
func real Volume(real[int] &X)
{
	Vh rho;
	rho[]=X;
	Vh rho3=rho*rho*rho;
	real res = 1./3.*int2d(Th)(rho3*sin(y));
	return res;
}
func real[int] GradVolume(real[int] &X)
{
	Vh rho;
	rho[]=X;
	varf dVolume(u,v) = int2d(Th)(rho*rho*sin(y)*v);
	real[int] grad = dVolume(0,Vh);
	return grad;
}
matrix hessianV;
func matrix HessianVolume(real[int] &X)
{
	Vh rho;
	rho[]=X;
	varf d2Volume(w,v) = int2d(Th)(2*rho*sin(y)*v*w);
	hessianV = d2Volume(Vh,Vh);
	return hessianV;
}



/*
 * If we want to use the volume as a constraint function
 * we must wrap it in some freefem functions returning the appropriate type 
 * The lagrangian hessian also have to be wrapped since the Volume is not linear with 
 * respect to rho, it will constribbute to the hessian.
 */
func real[int] ipVolume(real[int] &X) {real[int] vol = [Volume(X)]; return vol;}
matrix mdV;
func matrix ipGradVolume(real[int] &X) {real[int,int] dvol(1,Vh.ndof); dvol(0,:)=GradVolume(X); mdV=dvol; return mdV;}
matrix HLagrangian;
func matrix ipHessianLag(real[int] &X,real objfact,real[int] &lambda)
{
	HLagrangian = objfact*HessianArea(X) + lambda[0]*HessianVolume(X);
	return HLagrangian;
}
/*building struct for GradVolume */
int[int] gvi(Vh.ndof),gvj=0:Vh.ndof-1;
gvi=0;


/*
 * The starting value
 */
Vh rc=startshape ;


/*
 * Bounds definition
 */
Vh ub=1.e19;
//Vh lb=1.,ub=3.;


/*
 * make it funnier by puptting some objects inside the shape , where rho has to be greater than a given value
 */
Vh lb=0;
int shapeswitch=1;
real sigma=2*pi/40.,treshold=0.1;
func real Gaussian(real X,real Y,real theta,real phi)
{
	real deltax2 = square((X-theta)*sin(Y)) , deltay2 = square(Y-phi);
	return exp(-0.5 * (deltax2 + deltay2) / (sigma*sigma));
}
real e=0.1,r0=0.25,rr=2-r0;
real E=1./(e*e),RR=1./(rr*rr);
func disc1 = sqrt(1./(RR+(E-RR)*cos(y)*cos(y)))*(1+0.1*cos(7*x));
func disc2 = sqrt(1./(RR+(E-RR)*cos(x)*cos(x)*sin2))  ;
if(1) //doesn't work well...
{
	lb=r0;
	/*{
		func f = rr*Gaussian(x,y,0,0);
		func g = rr*Gaussian(x,y,0,pi);
		lb = max(max(lb,f),g);
	}*/
	for(int q=0;q<5;++q)
	{
		func f = rr*Gaussian(x,y,2*q*pi/5.,pi/3.);
		func g = rr*Gaussian(x,y,2*q*pi/5.+pi/5.,2.*pi/3.);
		lb = max(max(lb,f),g);
	}
	lb = max(lb,rr*Gaussian(x,y,2*pi,pi/3));
}
lb =  max(lb,max(disc1, disc2));
real Vobj = Volume(lb[]);
real Vnvc = 4./3.*pi*pow(lb[].linfty,3);

if(1) Plot3D(lb[],"object_inside",0);
real[int] clb=0.,cub=[(1-alpha)*Vobj + alpha*Vnvc];


/*
 * Call to IPOPT
 */
int res=IPOPT(Area,GradArea,ipHessianLag,ipVolume,ipGradVolume,rc[],ub=ub[],lb=lb[],clb=clb,cub=cub,checkindex=1,maxiter=kkk<nadapt-1 ? 40:150,warmstart=kkk,lm=lm,uz=uz[],lz=lz[],tol=0.00001,structjacc=[gvi,gvj]);
cout << " IPOPT: res =" << res << endl ;
/*
 * Display the final mesh
 */
Plot3D(rc[],"Shape_at_"+kkk,0);
Plot3D(GradArea(rc[]),"ShapeGradient",0);
if(kkk<nadapt-1)
{
	Th = adaptmesh(Th,rc*cos(x)*sin(y),rc*sin(x)*sin(y),rc*cos(y)/*,hmax=pi/np*/,nbvx=50000,periodic=[[2,y],[4,y]/*,[1,x],[3,x]*/]);
	plot(Th,wait=1);
	startshape = rc;
	uz=uz;//dual variables interpolation
	lz=lz;
}

// FFCS regression reference value
regtest=rc[]'*rc[];
}

