clear all;   
close all;
tic;
tol=1e-2;   %tolerance
tol2=1e-6;
load('xydat.rpt');
v=0.499;
Es=10000;
mu=Es/(1+v)/2;
lamda=2*mu*v/(1-2*v);

sigmay=100;
cohesion=sigmay/2;
phi=1e-8;
psi=1e-8;
hiso=0;
a=1;
b=0;
c=0;
% N number in of points in the radial direction, 
% M nummber of points in the circumferential direction,
% L number of harmonic term
%U=u1_k cos(k t)+u2_k sin(k t) radial displacements (same as erP,etP,ezP)
%V=-v1_k sin(k t)+v2_k cos(k t) circumferential displacements (same as
%ertP)
delta_mat=[0.3];
r0=1;rm=60;
L=5;N=20;nD=1;

%bc1=u at r0, bc2=v at r0, bc3=u at rm , bc4=v at rm

ri=logspace(log10(r0),log10(rm),N);rp=logspace(log10(r0),log10(rm),500);
M=2*L+1;t=linspace(0,2*pi,M+1);ti=t(1:M);rp=ri;
% r-theta =ri-ti
%phi(1)=phi_r,phi(2)=phi_theta,phi(3)=diff(phi_r),phi(4)=diff(phi_theta),
erE=zeros(N,M);etE=zeros(N,M);ertE=zeros(N,M);ezE=zeros(N,M);
erEc=zeros(N,M);etEc=zeros(N,M);ertEc=zeros(N,M);ezEc=zeros(N,M);
ezPc=zeros(N,M);erPc=zeros(N,M);etPc=zeros(N,M);ertPc=zeros(N,M);
qmat=zeros(N,M);U=zeros(N,M);V=zeros(N,M);

%  Fr1=zeros(L+1,N);Ft1=zeros(L,N);Fr2=zeros(L,N); Ft2=zeros(L+1,N);
% ferP1=zeros(L+1,N);fetP1=zeros(L+1,N);fertP1=zeros(L,N);fezP1=zeros(L,N);
% ferP2=zeros(L,N);fetP2=zeros(L,N);fertP2=zeros(L+1,N);fezP2=zeros(L+1,N); 
% derP=zeros(N,M);detP=zeros(N,M);dertP=zeros(N,M);dezP=zeros(N,M);

En=0.01;
disptot=0;
for ii=1:nD
    delta=delta_mat(ii);

%     if(delta>0.01)
%         delta=0.01
%     end
    bc_disp1=zeros(L,4);
     bc_disp1(1,1:2)=delta;
     bc_disp2=zeros(L,4);
%      bc_disp2(1,1:2)=delta;
%     Eold=zeros(2*L+1,2*N);
%     Enew=ones(2*L+1,2*N);
      Eold=ones(2*N,M)*0.001;
      Enew=ones(2*N,M);
    
    count=0;
    
  Fr1=zeros(L+1,N);Ft1=zeros(L,N);Fr2=zeros(L,N); Ft2=zeros(L+1,N);
  ferP1=zeros(L+1,N);fetP1=zeros(L+1,N);fertP1=zeros(L,N);fezP1=zeros(L,N);
  ferP2=zeros(L,N);fetP2=zeros(L,N);fertP2=zeros(L+1,N);fezP2=zeros(L+1,N); 
  derP=zeros(N,M);detP=zeros(N,M);dertP=zeros(N,M);dezP=zeros(N,M);
    
% Fr1=Fr1/2;Fr2=Fr2/2; Ft2=Ft2/2;
%     ferP1=ferP1/2;fetP1=fetP1/2;fertP1=fertP1/2;fezP1=fezP1/2;
%     ferP2=ferP2/2;fetP2=fetP2/2;fertP2=fertP2/2;fezP2=fezP2/2; 
%     derP=derP/2;detP=detP/2;dertP=dertP/2;dezP=dezP/2;
%    
    
     while(norm(Eold-Enew)/norm(Eold)>tol)
        count=count+1;
        Ep=Enew;
        u1=zeros(L+1,N);u2=zeros(L,N);v1=zeros(L,N);v2=zeros(L+1,N);
        du1dr=zeros(L+1,N);du2dr=zeros(L,N);dv1dr=zeros(L,N);
        dv2dr=zeros(L+1,N);
        solinit0=bvpinit(rp,[0.0001 1]);
        solinitk=bvpinit(rp,[0.0001 1 -1 1]);
        %solving for zero harmonic parallel load
        if(norm(Fr1(1,:))>tol2)
           sol= bvp4c(@(r,phi)phiode0(r,phi,ri,Fr1(1,:)),@phibc0,solinit0);
           xi0=deval(sol,ri);
           u1(1,:)=xi0(1,:);
           du1dr(1,:)=xi0(2,:);
        end

        if(norm(Ft2(1,:))>tol2)
           sol= bvp4c(@(r,phi)phiode0(r,phi,ri,Ft2(1,:)),@phibc0,solinit0);
           xi0=deval(sol,ri);
           v2(1,:)=xi0(1,:);
           dv2dr(1,:)=xi0(2,:);
        end
        kc=0;
        for k=1:L
            kc=kc+1;
            if((norm(Fr1(k+1,:))>tol2)||(norm(Ft1(k,:))>tol2)...
                    ||(norm(bc_disp1(k,:))>tol2))
                sol= bvp4c(@(r,phi)phiodek(r,phi,ri,k,v,Fr1(kc+1,:),...
                Ft1(kc,:)),@(phia,phib)phibck(phia,phib,bc_disp1(kc,:)),...
                solinitk);
                xik=deval(sol,ri);
                u1(kc+1,:)=xik(1,:);
                v1(kc,:)=xik(2,:);
                du1dr(kc+1,:)=xik(3,:);
                dv1dr(kc,:)=xik(4,:);
            end
        end
        kc=0;
        for k=1:L
            kc=kc+1;
            if((norm(Fr2(k,:))>tol2)||(norm(Ft2(k+1,:))>tol2||...
                    (norm(bc_disp2(k,:))>tol2)))
                sol= bvp4c(@(r,phi)phiodek(r,phi,ri,k,v,Fr2(kc,:),...
               Ft2(kc+1,:)),@(phia,phib)phibck(phia,phib,bc_disp2(kc,:)),...
               solinitk);
                xik=deval(sol,ri);
                u2(kc,:)=xik(1,:);
                v2(kc+1,:)=xik(2,:);
                du2dr(kc,:)=xik(3,:);
                dv2dr(kc+1,:)=xik(4,:);
            end
        end

      derE=(du1dr-ferP1).'*cos((0:L).'*ti)+(du2dr-ferP2).'*sin((1:L).'*ti);
        
      detE=(u1./repmat(ri,[L+1,1])-fetP1).'*cos((0:L).'*ti)+...
       (u2./repmat(ri,[L,1])-fetP2).'*sin((1:L).'*ti)...
       -(v1./repmat(ri,[L,1])).'*(cos((1:L).'*ti).*repmat((1:L).',[1,M]))....
      -(v2./repmat(ri,[L+1,1])).'*(sin((0:L).'*ti).*repmat((0:L).',[1,M]));
        
        
        dertE=(1/2*dv2dr-fertP2).'*cos((0:L).'*ti)-(1/2*dv1dr-fertP1).'*...
            sin((1:L).'*ti)-1/2*(u1./repmat(ri,[L+1,1])).'*(sin((0:L).'*...
            ti).*repmat((0:L).',[1,M]))+1/2*(u2./repmat(ri,[L,1])).'*...
            (cos((1:L).'*ti).*repmat((1:L).',[1,M]))+1/2*...
            (v1./repmat(ri,[L,1])).'*sin((1:L).'*ti)-1/2*...
            (v2./repmat(ri,[L+1,1])).'*cos((0:L).'*ti);
        
        for j=1:M
            for i=1:N
                epsEtr=[derE(i,j) detE(i,j) -dezP(i,j) dertE(i,j) 0 0].'....
                    +[erE(i,j) etE(i,j) ezE(i,j) ertE(i,j) 0 0].';
                epsP=[derP(i,j) detP(i,j) dezP(i,j) dertP(i,j) 0 0].'....
                    +[erPc(i,j) etPc(i,j) ezPc(i,j) ertPc(i,j) 0 0].';
                 [Dcons,sig,epsE,epsPn,sigmayn,q,itnum] =...
           Von_Mises_Isotropic_hardening(mu,lamda,sigmay,hiso,epsEtr,epsP);

                derP(i,j)=epsPn(1,1)-erPc(i,j);
                detP(i,j)=epsPn(2,1)-etPc(i,j);
                dezP(i,j)=epsPn(3,1)-ezPc(i,j);
                dertP(i,j)=epsPn(4,1)-ertPc(i,j);
                erEc(i,j)=epsE(1,1);
                etEc(i,j)=epsE(2,1);
                ezEc(i,j)=epsE(3,1);
                ertEc(i,j)=epsE(4,1); 
 
            end
        end
        for k=1:L
            ferP1(k+1,:)=2/M*sum((derP.*repmat(cos(k.*ti),[N,1])).');
            ferP2(k,:)=2/M*sum((derP.*repmat(sin(k.*ti),[N,1])).');
            
            fetP1(k+1,:)=2/M*sum((detP.*repmat(cos(k.*ti),[N,1])).');
            fetP2(k,:)=2/M*sum((detP.*repmat(sin(k.*ti),[N,1])).');
            
            fezP1(k+1,:)=2/M*sum((dezP.*repmat(cos(k.*ti),[N,1])).');
            fezP2(k,:)=2/M*sum((dezP.*repmat(sin(k.*ti),[N,1])).');
             
            fertP1(k,:)=-2/M*sum((dertP.*repmat(sin(k.*ti),[N,1])).');
            fertP2(k+1,:)=2/M*sum((dertP.*repmat(cos(k.*ti),[N,1])).');
        end
        ferP1(1,:)=1/M*sum(derP.');
        fetP1(1,:)=1/M*sum(detP.');
        fezP1(1,:)=1/M*sum(dezP.');
        fertP2(1,:)=1/M*sum(dertP.');
        
        for k=1:L
           Fr1(k+1,:)=dfdx(ferP1(k+1,:)+(v/(1-v)).*(fetP1(k+1,:)+...
               fezP1(k+1,:)),ri,N)+(1-2*v)./(1-v).*(ferP1(k+1,:)-...
               fetP1(k+1,:)-k.*fertP1(k,:))./ri;
           Ft1(k,:)=2.*k.*v./(1-2*v).*(ferP1(k+1,:)+fezP1(k+1,:))./ri...
               +2.*k.*(1-v)./(1-2*v).*fetP1(k+1,:)./ri+4.*fertP1(k,:)./ri...
               +2.*dfdx(fertP1(k,:),ri,N);
          Fr2(k,:)=dfdx(ferP2(k,:)+(v/(1-v)).*(fetP2(k,:)+fezP2(k,:)),ri,N)....
             +(1-2*v)./(1-v).*(ferP2(k,:)-fetP2(k,:)-k.*fertP2(k+1,:))./ri;
           Ft2(k+1,:)=2.*k.*v./(1-2*v).*(ferP2(k,:)+fezP2(k,:))./ri...
               +2.*k.*(1-v)./(1-2*v).*fetP2(k,:)./ri+4.*fertP2(k+1,:)./ri...
               +2.*dfdx(fertP2(k+1,:),ri,N);           
        end
        Fr1(1,:)=dfdx(ferP1(1,:)+(v/(1-v)).*fetP1(1,:)+(v/(1-v)).*...
            fezP1(1,:),ri,N)+(1-2*v)./(1-v).*(ferP1(1,:)-fetP1(1,:))./ri;
        Ft2(1,:)=4.*fertP2(1,:)./ri+2.*dfdx(fertP2(1,:),ri,N);  
        
        sig_r=2.*mu.*erEc+lamda.*(erEc+etEc+ezEc);
        sig_t=2.*mu.*etEc+lamda.*(erEc+etEc+ezEc);
        sig_z=2.*mu.*ezEc+lamda.*(erEc+etEc+ezEc);
        sig_rt=2.*mu.*ertEc;
        Px=-r0.*trapz(t,[sig_r(1,:) sig_r(1,1)].*cos(t)-...
            [sig_rt(1,:) sig_rt(1,1)].*sin(t));
        Py=-r0.*trapz(t,[sig_r(1,:) sig_r(1,1)].*sin(t)+...
            [sig_rt(1,:) sig_rt(1,1)].*cos(t));
%Ec=1/2*trapz(ri,ri.*trapz(ti,(sig_r.*erEc+sig_t.*etEc+sig_z.*ezEc+2.*sig_rt.*ertEc).'))-P*delta;
        deltaU=u1.'*cos((0:L).'*ti)+u2.'*sin((1:L).'*ti);
        deltaV=-v1.'*sin((1:L).'*ti)+v2.'*cos((0:L).'*ti);
%         Ec=[Fr1 Ft2;Fr2 Ft1];
        Ec=[deltaU;deltaV];

       
        Enew=Ec;Eold=Ep;
        if(norm(derP)+norm(detP)+norm(dertP)<=tol2) 
            Eold=Ec;
            
            
           
         end
 [ii,count,abs(norm(Eold-Enew)/norm(Eold)),Py/2/r0/cohesion,delta+disptot]
   
     end
   
    erE=erEc;etE=etEc;ertE=ertEc;ezE=ezEc;
    erPc=erPc+derP;etPc=etPc+detP;ertPc=ertPc+dertP;ezPc=ezPc+dezP;
    
     Force(ii,:)=[Px,Py];disptot=delta+disptot;
     disp(ii)=disptot;
     U=deltaU+U;
     V=deltaV+V;
    
end
% U=u1.'*cos((0:L).'*ti)+u2.'*sin((1:L).'*ti);
% V=-v1.'*sin((1:L).'*ti)+v2.'*cos((0:L).'*ti);
% 
% tf=linspace(0,2*pi,N);
% for i=1:N
%     Uinterp(i,:)=interp1(ti,U(i,:),tf);
%     Vinterp(i,:)=interp1(ti,V(i,:),tf);
% end

figure;
plot(disp/r0/2,Force(:,2)/2/r0/cohesion,'s');
hold;
plot(xydat(:,1)*0.5/r0/2,2*xydat(:,2)/r0/sigmay,'o');
time=toc