load "Curvature"
load "isoline"
load "iovtk"
verbosity=0;
string outputFileName = "output/"; 
int[int] orderOutF = [1,1,1];
int[int] orderOutS = [1,1,1,1];

int n, mh=3, m=15, NN=6000, saveEach=30;
real muf=1,c1=2.e6,penal=0.0,g=-0,rhof=1.e3,rhos=1.e3,rhod=rhos-rhof;
real T=6, dt=T/NN, mus=dt*c1-muf;
real x0=0.2, y0=0.2, r=0.05, L=2.5, H=0.41, h=0.02, l=0.35, theta=0.2013579208;
real x1=x0+r*cos(theta), y1=y0-h/2, x2=x1, y2=y0+h/2;
real xoo=0.6,yoo=y0;


// Fluid Region
border a1(t=0,L) {x=t; y=0 ;label=1;};
border a2(t=0,H) {x=L; y=t ;label=2;};
border a3(t=L,0) {x=t; y=H ;label=1;};
border a4(t=H,0) {x=0; y=t ;label=3;};
//Hole
border disc(t=0, 2*pi)  {x=x0+r*cos(t); y=y0+r*sin(t); label=4;};  
//lines
border l0(t=0.25,0.6) {x=t; y=y0;label=5;};
border l1(t=x2,0.6) {x=t; y=0.04*(x-x2)/l+y2;  label=5;};
border l2(t=x1,0.6) {x=t; y=-0.04*(x-x1)/l+y1; label=5;};

plot(a1(L*m/H)+a2(m)+a3(L*m/H)+a4(m)+disc(pi*mh/theta)+l0(l*mh/h)+l1(l*mh/h)+l2(l*mh/h), wait=1, dim=2);

mesh Th = buildmesh(a1(L*m/H)+a2(m)+a3(L*m/H)+a4(m)+disc(-pi*mh/theta)+l0(l*mh/h)+l1(l*mh/h)+l2(l*mh/h));
//plot(Th, wait=1);
// Solid Region
border b1(t=x1,0.6) {x=t; y=y1 ;label=5;};
border b2(t=y1,y2) {x=0.6; y=t ;label=5;};
border b3(t=0.6,x2) {x=t; y=y2 ;label=5;};
border b4(t=theta,-theta) {x=x0+r*cos(t); y=y0+r*sin(t);label=5;};
plot(b1(l*mh/h)+b2(mh)+b3(l*mh/h)+b4(mh), wait=1, dim=2);

mesh Ths = buildmesh(b1(l*mh/h)+b2(mh)+b3(l*mh/h)+b4(mh));
plot(Ths, Th, wait=1);

mesh Thso=Ths, Ths0=Ths;
fespace Vh(Th,P2);
fespace Ph(Th,P1);
fespace Vhs(Ths,P2);
fespace Vhso(Thso,P2);
fespace Rh(Th,[P2,P2,P1]);
fespace Rhs(Ths,[P2,P2]);

Ph p,ph;
Vh u,v,uh,vh,uold=0,vold=0,uu;
Vhs us,vs,ush,vsh,usold=0,vsold=0,d1=0,d2=0,xx=x,yy=y; 
Vhso uso,vso,do1,do2;;

macro div(u,v) ( dx(u)+dy(v) ) // EOM
macro DD(u,v)  [[2*dx(u),div(v,u)],[div(v,u),2*dy(v)]] // EOM
macro Grad(u,v)[[dx(u),dy(u)],[dx(v),dy(v)]] // EOM

int ioo;
for(int i=0; i<d2[].n; i++){
	if (sqrt((xx[][i]-xoo)^2+(yy[][i]-yoo))<1.e-6) ioo=i;
}
cout<<"======"<<ioo<<endl;

varf fluid([u,v,p],[uh,vh,ph]) =
	int2d(Th)(rhof*[u,v]'*[uh,vh]/dt- div(uh,vh)*p -div(u,v)*ph + penal*p*ph 
	+ muf/2*trace(DD(u,v)'*DD(uh,vh)))
	+ on(1,u=0, v=0) + on(3,u=12*y*(H-y)/H/H,v=0) + on(4,u=0,v=0);

varf resf([u,v,p],[uh,vh,ph]) =
	int2d(Th)(g*rhof*vh+rhof*[convect([uold,vold],-dt,uold),
	convect([uold,vold],-dt,vold)]'*[uh,vh]/dt )
	+ on(1,u=0, v=0) + on(3,u=12*y*(H-y)/H/H,v=0) + on(4,u=0,v=0);

varf solid([us,vs],[ush,vsh]) =
	int2d(Ths)( rhod*[us,vs]'*[ush,vsh]/dt
	+ mus/2*trace(DD(us,vs)'*DD(ush,vsh))
	- dt*c1*trace((Grad(us,vs)'*Grad(d1,d2)+Grad(d1,d2)'*Grad(us,vs))*Grad(ush,vsh)')) ;

varf ress([us,vs],[ush,vsh]) =	
	int2d(Ths)( g*rhod*vsh - c1*trace((DD(d1,d2) - Grad(d1,d2)'*Grad(d1,d2))*Grad(ush,vsh)')  
	+ rhod*[usold,vsold]'*[ush,vsh]/dt );

matrix A = fluid(Rh,Rh);

ofstream file("fdm.txt");
file.precision(16);


for(n=0;n<=NN;n++){

  	cout<<"FSI Time Step: "<<n<<endl;
	
	real[int] rhs1 = resf(0,Rh);

	real[int] rhs2 = ress(0,Rhs);
	matrix A = fluid(Rh,Rh);
	matrix B = solid(Rhs,Rhs);
	matrix P = interpolate(Rhs,Rh);
	real[int] rhs = P'*rhs2;
	rhs += rhs1;
	matrix T = P'*B;
	matrix AB = T*P;
	AB+=A;

	set(AB,solver=UMFPACK);

	Rh [w1, w2, wp];
	// Solve
	real[int] sol(Rh.ndof);
	sol= w1[];
	sol = AB^-1 * rhs;
	w1[]=sol; u=w1; v= w2; p=wp;

	if (n % saveEach == 0){
		int k=n/saveEach;
		savevtk(outputFileName+"fluid"+k+".vtk", Th, u,v,p, dataname="ux uy p", order=orderOutF);
		savevtk(outputFileName+"solid"+k+".vtk", Ths, us,vs,d1,d2, dataname="usx usy q dispx dispy", order=orderOutS);		
	}

	
	Thso=Ths;		
	uso=u; vso=v; do1=d1; do2=d2;
	
	Ths = movemesh(Thso, [x+uso*dt, y+vso*dt]);

	d1=0;  d1[]=do1[]+uso[]*dt; 
	d2=0;  d2[]=do2[]+vso[]*dt; 
	us=0;  us[]=uso[];
	vs=0;  vs[]=vso[];
	
	uold=u;vold=v; usold=us;vsold=vs;
	
	uu=sqrt(u^2+v^2);
	plot(uu,Ths,coef=0.1,fill=1,value=1,wait=0);


	file <<d2[][ioo]<< endl;	
}

file.flush;