load "ff-NLopt"

//A brief script to demonstrate how to use the freefemm interfaced nlopt routines
//The problem consist in solving a simple variational inequality using one of the
//optimization algorithm of nlopt. We restart the algorithlm a few times agter
//performing some mesh adaptation to get a more precise output.
int kas = 3; //    choose of the algo ...

int NN = 6;
mesh Th = square(NN,NN);
func f1=1.;
func f2=-1.;
func g1=0.;
func g2=0.1;
int iter=0;
int nadapt=2;
real starttol=1e-6,bctol=6.e-12;
fespace Vh(Th,P1);
Vh oldu1,oldu2;


for(int al=0;al<nadapt;++al)
{
	
varf BVF(v,w) = int2d(Th)(0.5*dx(v)*dx(w) + 0.5*dy(v)*dy(w));
varf LVF1(v,w) = int2d(Th)(f1*w);
varf LVF2(v,w) = int2d(Th)(f2*w);
matrix A =  BVF(Vh,Vh);
real[int] b1 = LVF1(0,Vh) , b2 = LVF2(0,Vh); 

varf Vbord(v,w) = on(1,2,3,4,v=1);
Vh In,Bord;
Bord[] = Vbord(0,Vh,tgv=1);
In[] = Bord[] ? 0:1;
Vh gh1=Bord*g1,gh2=Bord*g2;

func real J(real[int] &X)
{
	Vh u1,u2;
	u1[] = X(0:Vh.ndof-1);
	u2[] = X(Vh.ndof:2*Vh.ndof-1);
	iter++;
	real[int] Au1 = A*u1[], Au2 = A*u2[];
	Au1 -= b1;
	Au2 -= b2;
	real val = u1[]'*Au1 + u2[]'*Au2;
	if(iter %10==9) 
	plot(u1,u2,nbiso=30,fill=1,dim=3,cmm="adapt level " + al + " - iteration " + iter + " - J = " + val,value=1);
	return val;
}

varf dBFV(v,w) = int2d(Th)(dx(v)*dx(w)+dy(v)*dy(w));
matrix dA = dBFV(Vh,Vh);
func real[int] dJ(real[int] &X)
{
	Vh u1,u2;
	u1[] = X(0:Vh.ndof-1);
	u2[] = X(Vh.ndof:2*Vh.ndof-1);
	
	real[int] grad1 = dA*u1[], grad2 = dA*u2[];
	grad1 -= b1;
	grad2 -= b2;
	real[int] Grad(X.n);
	Grad(0:Vh.ndof-1) = grad1;
	Grad(Vh.ndof:2*Vh.ndof-1) = grad2; 
	return Grad;
}


func real[int] IneqC(real[int] &X)
{
	real[int] constraints(Vh.ndof);
	for(int i=0;i<Vh.ndof;++i) constraints[i] = X[i] - X[i+Vh.ndof];
	return constraints;
}
func real[int,int] dIneqC(real[int] &X)
{
	real[int,int] dconst(Vh.ndof,2*Vh.ndof);//so sparse... shame!
	dconst=0;
	for(int i=0;i<Vh.ndof;++i)
	{
		dconst(i,i) = 1.;
		dconst(i,i+Vh.ndof) = -1.;
	}
	return dconst;
}

real[int] BordIndex(Th.nbe); //Indexes of border d.f.
{
	int k=0;
	for(int i=0;i<Bord.n;++i) if(Bord[][i]) {BordIndex[k]=i; ++k;}
}
func real[int] BC(real[int] &X)
{
	real[int] bc(2*Th.nbe);
	for(int i=0;i<Th.nbe;++i)
	{
		int I = BordIndex[i];
		bc[i] = X[I] - gh1[][I];
		bc[i+Th.nbe] = X[I+Th.nv] - gh2[][I];
	}
	return bc;
}
func real[int,int] dBC(real[int] &X)
{
	real[int,int] dbc(2*Th.nbe,2*Th.nv);//yet sparser...:(
	dbc=0.;
	for(int i=0;i<Th.nbe;++i)
	{
		int I=BordIndex[i];
		dbc(i,I) = 1.;
		dbc(i+Th.nbe,I+Th.nv) = 1.;
	}
	return dbc;
}


real[int] start(2*Vh.ndof),up(2*Vh.ndof),lo(2*Vh.ndof);

if(al==0)
{
	start(0:Vh.ndof-1) = 0.;
	start(Vh.ndof:2*Vh.ndof-1) = 0.01;//comment this two lines and uncomment what follows to use ub and lb
	/*Vh g1i=g1,g2i=g2;
	start(0:Vh.ndof-1) = g1i[];
	start(Vh.ndof:2*Vh.ndof-1) = g2i[]; */
}
else 
{
	start(0:Vh.ndof-1) = oldu1[];
	start(Vh.ndof:2*Vh.ndof-1) = oldu2[]; 
}
up=1000000;
lo=-1000000;
for(int i=0;i<Vh.ndof;++i)
{
	if(Bord[][i])
	{
		up[i] = gh1[][i] + bctol;
		lo[i] = gh1[][i] - bctol;
		up[i+Vh.ndof] = gh2[][i] + bctol;
		lo[i+Vh.ndof] = gh2[][i] - bctol;
	}
}

//Uncomment this to use ub and lb
if(kas<3){
  real[int] diff1 = start - lo , diff2 = up - start;
  for(int i=0;i<diff1.n;++i) 
   {
	if(diff1[i]<0) start[i]=lo[i];
	if(diff2[i]<0) start[i]=up[i]; 
   }
}

real mini=1e100;
if(kas==1)
   mini = nloptAUGLAG(J,start,grad=dJ,lb=lo,ub=up,IConst=IneqC,gradIConst=dIneqC,subOpt="LBFGS",stopMaxFEval=10000,stopAbsFTol=starttol);
else if(kas==2) 
 mini = nloptMMA(J,start,grad=dJ,lb=lo,ub=up,stopMaxFEval=10000,stopAbsFTol=starttol);
else if(kas==3) 
 mini = nloptAUGLAG(J,start,grad=dJ,IConst=IneqC,gradIConst=dIneqC,EConst=BC,gradEConst=dBC,subOpt="LBFGS",stopMaxFEval=200,stopRelXTol=1e-2);
else if(kas==4) 
 mini = nloptSLSQP(J,start,grad=dJ,IConst=IneqC,gradIConst=dIneqC,EConst=BC,gradEConst=dBC,stopMaxFEval=10000,stopAbsFTol=starttol);
Vh best1,best2;
best1[] = start(0:Vh.ndof-1);
best2[] = start(Vh.ndof:2*Vh.ndof-1);

Th = adaptmesh(Th,best1,best2,err=0.05);

oldu1=best1;
oldu2=best2;
}
