clc; clear
addpath('..\..\..\SharedFiles');
sideLengthX = 1;
sideLengthY = 1;
ndivx  = 101;
ndivy  = 101;
dx = sideLengthX/ndivx;
dy = sideLengthY/ndivy;
maxdx = max(dx, dy);
x = linspace(0, sideLengthX, ndivx+1);
y = linspace(0, sideLengthY, ndivy+1);
[x, y] = meshgrid(x, y);
coord  = [x(:), y(:)];
thickness = 1;
particleVolume = (dx*dy*thickness) * ones(size(coord,1),1);
mvalue = 3.0;
delta = maxdx * (mvalue+0.015);
[ih, jh] = gridBasedNonUniformHorizonSearch(coord, delta, 1.5, 'no');
lgc = sum(coord.^2,2)>=(sideLengthX/2)^2;
% [coord, particleVolume, ih, jh, accumPos] = modifiedModel(lgc, coord, particleVolume, ih, jh);
[np, nd] = size(coord);

%
xi  = coord(jh,:)-coord(ih,:);
bl0 = vecnorm(xi,2,2);
vc = (bl0<delta-maxdx/2)*1 + ...
    (bl0>delta-maxdx/2).*(delta-bl0+maxdx/2)/dx;
vj = particleVolume(jh).*vc;
wf = exp(-bl0.^2/delta^2);
wfvj = wf.*vj;
ap = [0;find(diff(ih)==1); numel(ih)];
g1 = firstOrderGfun2D(xi, wfvj, ap);
g2 = secondOrderGhatfun2D(xi, wfvj, ap);

loadDisp = [0,0.001,0.002:0.00001:0.01];
scr = 0.01;
fail = ~preCrack2D(coord, ih, jh, [-0.5,0.5], [0.5,0.5]);
dmg0 = 1 - accumarray(ih, fail.*wfvj) ./ accumarray(ih, wfvj);
characteristicLength = sideLengthY*0.01;
resFolder = sprintf('tensile_crack_result\\lb_%gm\\', characteristicLength);
if(~exist(resFolder,'dir'))
    mkdir(resFolder)
end

emod = 12e6;
nu   = 0.2;

lambda = nu*emod/(1-2*nu)/(1+nu);
mu     = emod/(2*(1+nu));
mu_r   = 1/3 * mu;
gamma  = 4 * mu * characteristicLength^2;

init.x    = x;
init.y    = y;
init.wfvj = wfvj;
init.g    = g1;
init.ghat = g2;
init.dmg0 = dmg0;
init.emod = emod;
init.mu   = mu;
init.mu_r = mu_r;
init.lb   = characteristicLength;
init.gamma = gamma;
init.nu = nu;
save([resFolder,'init'], 'init');

for tt = 1:numel(loadDisp)
    
    g1_ = repmat(wfvj.*fail, 1, size(g1,2)) .* g1;
    g2_ = repmat(wfvj.*fail, 1, size(g2,2)) .* g2;
    
    g11 = (mu+mu_r)*(g2_(:,1)+g2_(:,2)) + (mu-mu_r)*(2*g2_(:,1));
    g12 = + (mu-mu_r)*g2_(:,3);
    g21 = + (mu-mu_r)*g2_(:,3);
    g22 = (mu+mu_r)*(g2_(:,1)+g2_(:,2)) + (mu-mu_r)*(2*g2_(:,2));
    
    gq1 = (g11.*xi(:,2)-g12.*xi(:,1))*0.5;
    gq2 = (g21.*xi(:,2)-g22.*xi(:,1))*0.5;
    qtgq = (xi(:,2).*gq1 - xi(:,1).*gq2)*0.5;
    
    g10 = g1_(:,1);
    g01 = g1_(:,2);
    
    gtheta = (g10.*xi(:,2)-g01.*xi(:,1))*0.5;
    ggamma = gamma*(g2_(:,1)+g2_(:,2));
    
    
    B = + sparse(ih, 3*jh-2, g10, np, 3*np) + sparse(ih, 3*jh-1, g01, np, 3*np) ...
        - sparse(ih, 3*ih-2, g10, np, 3*np) - sparse(ih, 3*ih-1, g01, np, 3*np)...
        + sparse(ih, 3*jh-0, gtheta, np, 3*np)...
        + sparse(ih, 3*ih-0, gtheta, np, 3*np); % 仍是加号原因在于平均转角
    
    % 先中间，后两边，先上后下
    D = + sparse(3*jh-2, 3*jh-2, g11, 3*np, 3*np) + sparse(3*jh-2, 3*jh-1, g12, 3*np, 3*np) ...
        + sparse(3*jh-1, 3*jh-2, g21, 3*np, 3*np) + sparse(3*jh-1, 3*jh-1, g22, 3*np, 3*np) ...
        - sparse(3*ih-2, 3*jh-2, g11, 3*np, 3*np) - sparse(3*ih-2, 3*jh-1, g12, 3*np, 3*np) ...
        - sparse(3*ih-1, 3*jh-2, g21, 3*np, 3*np) - sparse(3*ih-1, 3*jh-1, g22, 3*np, 3*np) ...
        - sparse(3*jh-2, 3*ih-2, g11, 3*np, 3*np) - sparse(3*jh-2, 3*ih-1, g12, 3*np, 3*np) ...
        - sparse(3*jh-1, 3*ih-2, g21, 3*np, 3*np) - sparse(3*jh-1, 3*ih-1, g22, 3*np, 3*np) ...
        + sparse(3*ih-2, 3*ih-2, g11, 3*np, 3*np) + sparse(3*ih-2, 3*ih-1, g12, 3*np, 3*np) ...
        + sparse(3*ih-1, 3*ih-2, g21, 3*np, 3*np) + sparse(3*ih-1, 3*ih-1, g22, 3*np, 3*np) ...
        ...
        + sparse(3*jh-2, 3*jh, gq1, 3*np, 3*np) + sparse(3*jh-1, 3*jh, gq2, 3*np, 3*np) ...
        + sparse(3*jh-2, 3*ih, gq1, 3*np, 3*np) + sparse(3*jh-1, 3*ih, gq2, 3*np, 3*np) ...
        - sparse(3*ih-2, 3*jh, gq1, 3*np, 3*np) - sparse(3*ih-1, 3*jh, gq2, 3*np, 3*np) ...
        - sparse(3*ih-2, 3*ih, gq1, 3*np, 3*np) - sparse(3*ih-1, 3*ih, gq2, 3*np, 3*np) ...
        ...
        + sparse(3*jh, 3*jh-2, gq1, 3*np, 3*np) + sparse(3*jh, 3*jh-1, gq2, 3*np, 3*np) ...
        - sparse(3*jh, 3*ih-2, gq1, 3*np, 3*np) - sparse(3*jh, 3*ih-1, gq2, 3*np, 3*np) ...
        + sparse(3*ih, 3*jh-2, gq1, 3*np, 3*np) + sparse(3*ih, 3*jh-1, gq2, 3*np, 3*np) ...
        - sparse(3*ih, 3*ih-2, gq1, 3*np, 3*np) - sparse(3*ih, 3*ih-1, gq2, 3*np, 3*np) ...
        ...
        + sparse(3*jh, 3*jh, qtgq, 3*np, 3*np) + sparse(3*jh, 3*ih, qtgq, 3*np, 3*np) ...
        + sparse(3*ih, 3*jh, qtgq, 3*np, 3*np) + sparse(3*ih, 3*ih, qtgq, 3*np, 3*np) ...
        ...
        + sparse(3*jh, 3*jh, ggamma, 3*np, 3*np) - sparse(3*jh, 3*ih, ggamma, 3*np, 3*np) ...
        - sparse(3*ih, 3*jh, ggamma, 3*np, 3*np) + sparse(3*ih, 3*ih, ggamma, 3*np, 3*np);
    
    
    K = (lambda-mu+mu_r)*(B'*B) + D;
    meps      = dx / 100;
    upNode    = find(coord(:,2)- sideLengthY > 0-meps);
    downNode  = find(coord(:,2) < 0 + meps);
    leftNode  = find(coord(:,1)< 0+meps & coord(:,2)- sideLengthY <= 0-eps & coord(:,2) >= meps);
    rightNode = find(coord(:,1) > sideLengthX-meps & coord(:,2)- sideLengthY <= 0-eps & coord(:,2) >= +meps);
    
    
    G = [...
        sparse((1:numel(upNode)),   3*upNode-1,        ones(size(upNode)),   numel(upNode), 3*np)
        sparse((1:numel(downNode)), 3*downNode-2,       ones(size(downNode)),   numel(downNode), 3*np);...
        sparse((1:numel(downNode)), 3*downNode,       ones(size(downNode)),   numel(downNode), 3*np);...
        sparse((1:numel(downNode)), 3*downNode-1,     ones(size(downNode)),  numel(downNode), 3*np); ...
        sparse((1:numel(upNode)),   3*upNode,        ones(size(upNode)),   numel(upNode), 3*np); ...
        sparse((1:numel(upNode)),   3*upNode-2,        ones(size(upNode)),   numel(upNode), 3*np)];
    
    %     Kdof = [ 3*upNode-1; 3*downNode-2; 3*downNode; 3*downNode-1; 3*upNode; 3*upNode-2];
    %     Tdof = (1:3*np)';
    %     Udof = Tdof;
    %     Udof(Kdof) = [];
    %     Uk   = [loadDisp(tt)*ones(size(upNode));
    %         zeros(size(downNode));
    %         zeros(size(downNode));
    %         zeros(size(downNode));
    %         zeros(size(upNode));
    %         zeros(size(upNode))
    %         ];
    %     F = zeros(3*np,1);
    %     Kuu = K(Udof, Udof);
    %     Kuk = K(Udof, Kdof);
    %     U = zeros(size(F));
    %     Fu = F(Udof);
    %     U(Kdof) = Uk;
    %     U(Udof) = (Fu - Kuk*Uk) \ Kuu;
    
    Fb = zeros(size(G,1),1);
    Fb(1:numel(upNode),1) = loadDisp(tt);
    nG = size(G,1);
    F = [zeros(3*np,1); Fb];
    A = [K,G';G,sparse(size(G,1),size(G,1))];
    b = F;
    
%     tic
%     if(tt==1)
%         [P, R, C] = equilibrate(A);
%         Anew = R*P*A*C;
%         bnew = R*P*b;
%         q = dissect(Anew);
%         Anew = Anew(q,q);
%         bnew = bnew(q);
%         [L,U] = ilu(Anew);
%     end
    
%     tol = 1e-10;
%     maxit = 50;
%     restart = 30;
%     if(tt==1)
%         [xnew, flag, relres] = gmres(Anew,bnew,restart,tol,maxit,L,U);
%     else
%         [xnew, flag, relres] = gmres(Anew,bnew,restart,tol,maxit,L,U, Ux*(tt+1)/(tt));
%     end
%     Ux = zeros(size(b));
%     Ux(q) = xnew;
%     Ux = C*Ux(:);
%     fprintf('Iteration\n')
%     toc
%     U1 = Ux(1:3:3*np);
%     U2 = Ux(2:3:3*np);
%     W3 = Ux(3:3:3*np);
%     eta = [...
%         ((U1(jh)-U1(ih))+xi(:,2).*(W3(ih)+W3(jh))/2), ...
%         ((U2(jh)-U2(ih))-xi(:,1).*(W3(ih)+W3(jh))/2)];
%     bl = vecnorm(eta+xi,2,2);
%     s = (bl-bl0)./bl0;
%     max(s.*fail)
    
    
    fprintf('Direct\n')
    tic
    Ux   = A\F;
    toc
    U1 = Ux(1:3:3*np);
    U2 = Ux(2:3:3*np);
    W3 = Ux(3:3:3*np);
    eta = [...
        ((U1(jh)-U1(ih))+xi(:,2).*(W3(ih)+W3(jh))/2), ...
        ((U2(jh)-U2(ih))-xi(:,1).*(W3(ih)+W3(jh))/2)];
    bl = vecnorm(eta+xi,2,2);
    s = (bl-bl0)./bl0;
    max(s.*fail)
    fail = fail & s<scr;
    dmg = 1 - accumarray(ih, fail.*wfvj) ./ accumarray(ih, wfvj);
    figure(100); clf
    h = pcolor(x,y,reshape(dmg,size(x)));
    set(h, 'edgecolor', 'none');
    colormap(jet(20)); colorbar; axis('equal')
    pause(0.0001)
    step.U1 = U1;
    step.U2 = U2;
    step.W3 = W3;
    step.dmg = dmg;
    step.applyU1 = 0;
    step.applyU2 = loadDisp(tt);
    Rf = K*Ux(1:3*np);
    step.Rf1 = -sum(Rf(3*downNode-1))*dx^3;
    step.Rf2 = -sum(Rf(3*downNode-2))*dx^3;
    step.Rc3 = -sum(Rf(3*downNode))*dx^3;
    save([resFolder,sprintf('step%d', tt)], 'step');
    

end
rmpath('..\..\..\SharedFiles');

figure
h = pcolor(x,y,reshape(U2,size(x)));
set(h, 'edgecolor', 'none');
colormap(jet(20)); colorbar; axis('equal')

