load "ff-Ipopt";

int NN=10;

mesh Th = square(NN,NN);
fespace Vh(Th,P1);
{  // verif no thresholding in operation 1*A; 
	macro grad(u) [dx(u),dy(u)]//
	varf a(u,v) = int2d(Th)(grad(u)'*grad(v));
	matrix A = a(Vh,Vh);
	matrix A1 = 1.*A;
	assert(A.nnz == A1.nnz ); 
}  // end verification 


func f=1;
func g=0;

varf Poiss(u,v) = int2d(Th)(0.5*dx(u)*dx(v) + 0.5*dy(u)*dy(v)) - int2d(Th)(v*f);
varf dPoiss(u,v) = int2d(Th)(dx(u)*dx(v) + dy(u)*dy(v)) ;
matrix A = Poiss(Vh,Vh);
matrix dA = dPoiss(Vh,Vh);
real[int] b = Poiss(0,Vh);

varf Bord(u,v) = int1d(Th,1,2,3,4)(0.5*u*v) - int1d(Th,1,2,3,4)(v*g);
varf dBord(u,v) = int1d(Th,1,2,3,4)(u*v);
real intg2 = int1d(Th,1,2,3,4)(0.5*g*g);


matrix M = dBord(Vh,Vh);
real[int] bc= Bord(0,Vh);



//cout << M << endl;
//matrix M = Bord(Vh,Vh);

int iter=0;

func real J(real[int] &X)
{
	Vh u;
	u[]=X;
	real[int] Au = A*X;
	Au += b;
	real res = X '* Au;
	plot(u,dim=3,fill=1,cmm="res = "+res+" - iter="+(iter++));
	return res;
}

int nj=0;
func real[int] dJ(real[int] &X)
{
	real[int] Au = dA*X;
	//Au *= 2.;
	Au += b;
	return Au;
}

int nh=0;

matrix MH;
func matrix HJ(real[int] &X,real objfact,real[int] &lm) 
{
	int[int] ia(0),ja(0),ic(0),jc(0);
	real[int] ca(0),cc(0);
	[ia,ja,ca] = dA;
	[ic,jc,cc] = M;
	ca *= (objfact ? objfact : 0.0001);
	cc *= (lm[0] ? lm[0] : 0.0001);
	matrix odA = [ia,ja,ca];
	matrix lmM = [ic,jc,cc];
	MH = odA + lmM;
	return MH;
}
//func matrix HJ(real[int] &X) {return dA;}
//out << dA << end;
int nc=0;
func real[int] C(real[int] &X)
{
	real[int] res=M*X;
	res *= 0.5;
	res += bc;
	real result = X '* res;
	result += intg2;
	real[int] rrr = [result];
	return rrr;
}

int ndc=0;
matrix MdC;
for(int i=0;i<Vh.ndof;++i)
{
	Vh xx=0.;
	xx[][i] = 1.;
	real[int] Mxx = M*xx[];
	real[int,int] MMxx(1,Mxx.n);
	MMxx(0,:) = Mxx;
	matrix MMMxx = MMxx;
	matrix temp = MMMxx;
	if(i==0) MdC = MMMxx;
	else MMMxx = MdC + temp;
	MdC = MMMxx;
}
cout << MdC << endl;
int[int] imdc(0),jmdc(0);
real[int] cmdc(0);
[imdc,jmdc,cmdc] = MdC;
func matrix dC(real[int] &X)
{
	real[int] res = M*X;
	res += bc;
	real[int,int] mdc(1,X.n);
	mdc(0,:) = res;
	for(int i=0;i<res.n;++i) if(res[i]) MdC(0,i) = res[i];
	
	return MdC;
}
//cout << borders << endl;
real[int] Clb(1),Cub(1);
Clb=0.;
Cub=0.;
Vh start=0.;//x*(x-1)*y*(1-y);

real[int] xub(Vh.ndof),xlb(Vh.ndof);
xub=1.e19;
xlb=-1.e19;
//xub = xub .* inside;
//xlb = xlb .* inside;
//cout << "J=" << J(start[]) << endl << " dJ=" << dJ(start[]) << endl << " HJ=" << HJ(start[]) << endl << " C=" << C(start[]) << endl << " dC=" << dC(start[]) << endl;
//cout << "dC = " << dC(start[]) << endl;

//IPOPT(J,dJ,HJ,start[],ConstFunc=C,ConstGrad=dC,clb=Clb,cub=Cub);
/*
real[int] lll=[0.];
cout << HJ(start[],1.,lll) << endl;
lll[0] = 1.;
cout << HJ(start[],0.,lll) << endl;*/


IPOPT(J,dJ,HJ,C,dC,start[],ub=xub, lb=xlb,cub=Cub,clb=Clb);

Vh uff,v;
solve LaplaceFF(uff,v) = int2d(Th)(dx(uff)*dx(v) + dy(uff)*dy(v)) - int2d(Th)(v*f) + on(1,2,3,4,uff=0);
real nl2 = sqrt(int2d(Th)(square(uff-start))), ninf = sqrt(int2d(Th)(square(dx(uff)-dx(start)) + square(dy(uff)-dy(start))));
cout << "||ipopt - ff||_2 = " << nl2 << "   ||grad||_2 = "  << ninf << endl;
