clc; clear
addpath('..\..\..\SharedFiles');
sideLengthX = 1;
sideLengthY = 1;
ndivx = 50;
ndivy = 50;
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(:)];
[np, nd] = size(coord);
thickness = 1;
particleVolume = (dx*dy*thickness) * ones(np,1);
delta = maxdx * 1.015;
[ih, jh] = gridBasedNonUniformHorizonSearch(coord, delta, 1.5, 'no');
xi  = coord(jh,:)-coord(ih,:);
bl0 = vecnorm(xi,2,2);
vc = (bl0<delta-maxdx/2)*1 + ...
    (bl0>delta-maxdx/2).*(delta-bl0+maxdx/2)/maxdx;
vj = particleVolume(jh).*vc;
wf = exp(-bl0.^2/delta^2);
wfvj = wf.*vj;
ap = [0;find(diff(ih)==1); numel(ih)];
G = secondOrderPDDOfun(xi, ap, wfvj, 2);
g1 = G(:,1:2);
g1 = firstOrderGfun2D(xi, wfvj, ap);
g1_ = repmat(wfvj, 1, size(g1,2)) .* g1;

emod = 1;
nu   = 0.2;
% nu = nu/(1+nu);
% emod = emod*(1+nu^2);
lambda = nu*emod/(1-2*nu)/(1+nu);
mu     = emod/(2*(1+nu));
% couplingNumber = sqrt(0.5);
couplingNumber = 0.5;
mu_r   = couplingNumber^2/(1-couplingNumber^2) * mu;
characteristicLength = 1;
gamma  = 2 * mu * characteristicLength^2;
microInertia = 1;

g10 = g1_(:,1);
g01 = g1_(:,2);


x11 = +g10 .* xi(:,2) * 0.5;
x12 = -g10 .* xi(:,1) * 0.5;
x21 = +g01 .* xi(:,2) * 0.5;
x22 = -g01 .* xi(:,1) * 0.5;

u = [0.01*coord(:,1) + 0.02*coord(:,2), 0.03*coord(:,1) + 0.04*coord(:,2)];
%theta = 0.0123*coord(:,1) + 0.0555*coord(:,2);
theta = 0.1*ones(size(coord(:,1)));
U = [u, theta]';
U = U(:);
Bxx = sparse(ih, 3*jh-2, g10, np, 3*np) - sparse(ih, 3*ih-2, g10, np, 3*np) ...
    + sparse(ih, 3*jh-0, x11, np, 3*np) + sparse(ih, 3*ih-0, x11, np, 3*np);
Byy = sparse(ih, 3*jh-1, g01, np, 3*np) - sparse(ih, 3*ih-1, g01, np, 3*np) ...
    + sparse(ih, 3*jh-0, x22, np, 3*np) + sparse(ih, 3*ih-0, x22, np, 3*np);
Bxy = sparse(ih, 3*jh-1, g10, np, 3*np) - sparse(ih, 3*ih-1, g10, np, 3*np) ...
    + sparse(ih, 3*jh-0, x12, np, 3*np) + sparse(ih, 3*ih-0, x12, np, 3*np);
Byx = sparse(ih, 3*jh-2, g01, np, 3*np) - sparse(ih, 3*ih-2, g01, np, 3*np) ...
    + sparse(ih, 3*jh-0, x21, np, 3*np) + sparse(ih, 3*ih-0, x21, np, 3*np);
Bxz = sparse(ih, 3*jh-0, g10, np, 3*np) - sparse(ih, 3*ih-0, g10, np, 3*np);
Byz = sparse(ih, 3*jh-0, g01, np, 3*np) - sparse(ih, 3*ih-0, g01, np, 3*np);

% 
% exy = Bxy * U




K = (lambda+2*mu)*(Bxx'*Bxx) + (lambda+2*mu)*(Byy'*Byy) + lambda*(Bxx'*Byy) + lambda*(Byy'*Bxx) ...
    + (mu+mu_r)*(Bxy'*Bxy) + (mu+mu_r)*(Byx'*Byx) + (mu-mu_r)*(Bxy'*Byx) + (mu-mu_r)*(Byx'*Bxy) + ...
    + gamma*(Bxz'*Bxz) + gamma*(Byz'*Byz);


int_xi2 = ((pi^(3/2)*erf(1))/2 - pi*exp(-1))*delta^3;
beta_u = 2 * mu / int_xi2;
beta_t = 2 * gamma / int_xi2;
ip = 1:np;
alpha_11 = sparse(ip, ip, accumarray(ih, wfvj.*xi(:,1).^2));
alpha_22 = sparse(ip, ip, accumarray(ih, wfvj.*xi(:,2).^2));
alpha_12 = sparse(ip, ip, accumarray(ih, wfvj.*xi(:,2).*xi(:,1)));
Ks_t = Bxx'*alpha_11*Bxx + Byx'*alpha_11*Byx + ...
    Bxy'*alpha_22*Bxy + Byy'*alpha_22*Byy ;%+ ...
    %2*Bxx'*alpha_12*Bxy + 2*Byx'*alpha_12*Byy;
    
meps      = maxdx / 100;
upNode    = find(coord(:,2)-1 > -meps);
downNode  = find(coord(:,2) < meps);
leftNode  = find(coord(:,1)<0+meps & coord(:,2)<1-meps & coord(:,2)>meps);
rightNode = find(coord(:,1)>1-meps & coord(:,2)<1-meps & coord(:,2)>meps);
G = [...
    sparse((1:numel(upNode)),   3*upNode-2,   ones(size(upNode)),    numel(upNode),   3*np); ...
    sparse((1:numel(upNode)),   3*upNode-1,   ones(size(upNode)),    numel(upNode),   3*np); ...
    sparse((1:numel(upNode)),   3*upNode ,    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-1, ones(size(downNode)),  numel(downNode), 3*np);...
    sparse((1:numel(downNode)), 3*downNode,   ones(size(downNode)),  numel(downNode), 3*np)];
F = zeros(3*np+size(G,1),1);
F(3*np+1:1:3*np+numel(upNode),1) = 1;
U = [K,G';G,sparse(size(G,1),size(G,1))]\F;
U1 = U(1:3:3*np);
U2 = U(2:3:3*np);
Omega3 = U(3:3:3*np);

% get stress and couple stress
theta_ave = (Omega3(jh) + Omega3(ih))*0.5;
eta = [U1(jh), U2(jh)] - [U1(ih), U2(ih)] + [xi(:,2).*theta_ave, -xi(:,1).*theta_ave];
exx = accumarray(ih, eta(:,1).*g10);
exy = accumarray(ih, eta(:,2).*g10);
eyy = accumarray(ih, eta(:,2).*g01);
eyx = accumarray(ih, eta(:,1).*g01);

figure(1); clf
h = pcolor(x,y,reshape(U2,size(x)));
set(h, 'edgecolor', 'none','facecolor', 'interp')
colormap(jet)
axis equal
title('U_2')
colorbar

figure(2); clf
h = pcolor(x,y,reshape(Omega3,size(x)));
set(h, 'edgecolor', 'none','facecolor', 'interp')
colormap(jet)
axis equal
title('\Omega_3')
colorbar

figure(3); clf
h = pcolor(x,y,reshape(U1,size(x)));
set(h, 'edgecolor', 'none','facecolor', 'interp')
colormap(jet)
axis equal
colorbar
title('U_1')

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


fprintf('max(U2)=%f\n', max(U2))
fprintf('min(Omega3) = %f\n', min(Omega3));
figure(5); clf
psel = abs(coord(:,1)-0.5) < 1e-5;
plot(coord(psel,2), Omega3(psel));
% 0p005
% min(U1)=-0.254971
% min(Omega3) = -0.001833
% 0p01
% min(U1)=-0.256106
% min(Omega3) = -0.001783
