clc;clear
% inpFileName = 'CirC2D8.inp';
% [node, element, ET] = inpProcess(inpFileName);
[node, element] =  c2d8([1,1], [10,10]);

N = @(xi,eta)[
    -1/4*(1-xi)*(1-eta)*(1+xi+eta),...
    -1/4*(1+xi)*(1-eta)*(1-xi+eta),...
    -1/4*(1+xi)*(1+eta)*(1-xi-eta),...
    -1/4*(1-xi)*(1+eta)*(1+xi-eta),...
    1/2*(1-xi^2)*(1-eta),...
    1/2*(1-eta^2)*(1+xi),...
    1/2*(1-xi^2)*(1+eta),...
    1/2*(1-eta^2)*(1-xi),...
    ];
diffN = @(xi,eta)[
    -((eta + 2*xi)*(eta - 1))/4, ((eta - 2*xi)*(eta - 1))/4, ((eta + 2*xi)*(eta + 1))/4, -((eta - 2*xi)*(eta + 1))/4,...
    xi*(eta - 1), 1/2 - eta^2/2, -xi*(eta + 1), eta^2/2 - 1/2;
    -((2*eta + xi)*(xi - 1))/4, ((xi + 1)*(2*eta - xi))/4, ((2*eta + xi)*(xi + 1))/4, -((xi - 1)*(2*eta - xi))/4,...
    xi^2/2 - 1/2, -eta*(xi + 1), 1/2 - xi^2/2, eta*(xi - 1)
    ];

GP = [-1/sqrt(3), 1/sqrt(3)];
W = [1,1];
ndof = 3;

emod = 1;
pr   = 0.2;
Lambda = pr*emod/(1-2*pr)/(1+pr);
G = emod/2/(1+pr);
coupling_number = 0.5;
Gc = coupling_number^2/(1-coupling_number^2)*G;
lc = 1;

De = [
    Lambda+2*G,      Lambda,     0,     0,          0,         0;
    Lambda ,     Lambda+2*G,     0,     0,          0,         0;
    0,                    0,  G+Gc,  G-Gc,          0,         0;
    0,                    0,  G-Gc,  G+Gc,          0,         0;
    0,                    0,     0,     0,   2*G*lc^2,         0;
    0,                    0,     0,     0,          0,  2*G*lc^2;
    ];

Ktot = zeros(size(node,1)*ndof);
for i = 1:1:size(element,1)
    enode = element(i,:);
    ecoor = node(enode,:);
    Ke = zeros(numel(enode)*ndof);
    nPos = reshape([3*enode-2;3*enode-1;3*enode],numel(enode)*ndof,1);
    for j1 = 1:1:numel(GP)
        for j2 = 1:1:numel(GP)
            xi = GP(j1);
            eta = GP(j2);
            H1 = W(j1);
            H2 = W(j2);
            Nval = N(xi,eta);
            dN = diffN(xi,eta);
            Jacobi = dN*ecoor;
            invJ = inv(Jacobi);
            detJ = det(Jacobi);
            pNpX = invJ*dN;
            B = zeros(6,24);
            B(1,1:3:end) = pNpX(1,1:1:end);
            B(2,2:3:end) = pNpX(2,1:1:end);
            B(3,2:3:end) = pNpX(1,1:1:end);
            B(3,3:3:end) = -Nval;
            B(4,1:3:end) = pNpX(2,1:1:end);
            B(4,3:3:end) = Nval;
            B(5,3:3:end) = lc*pNpX(1,1:1:end);
            B(6,3:3:end) = lc*pNpX(2,1:1:end);
            Ke = Ke + B'*De*B*H1*H2*detJ;
        end
    end
    Ktot(nPos,nPos) = Ktot(nPos,nPos) + Ke;
end

F = zeros(size(Ktot,1),1);

% pset1 = find(node(:,2)==-0.5);
% pset2 = find(node(:,1)==-0.5);
% pset3 = find(node(:,2)==0.5);
% pset4 = find(node(:,1)==0.5);
% 
% uc = [
%     pset1, 2*ones(size(pset1)),zeros(size(pset1));
%     pset1, 3*ones(size(pset1)),zeros(size(pset1));
%     pset2, 1*ones(size(pset2)),zeros(size(pset2));
%     pset2, 3*ones(size(pset2)),zeros(size(pset2));
%     pset3, 3*ones(size(pset3)),zeros(size(pset3));
%     pset3, 1*ones(size(pset3)),1*ones(size(pset3));
%     pset4, 3*ones(size(pset4)),0*ones(size(pset4));
%     ];
% 
% % for ii = 1:1:size(uc,1)
% %     i = ndof*(uc(ii,1)-1)+uc(ii,2);
% %     F(i,1) = uc(ii,3);
% %     for j = 1:1:size(Ktot,1)
% %         if(j~=i)
% %             F(j,1) = F(j,1) - Ktot(j,i)*uc(ii,3);
% %         end
% %     end
% %     Ktot(i,i)=1;
% %     for j = 1:1:size(Ktot,1)
% %         if(j~=i)
% %             Ktot(j,i) = 0; 
% %             Ktot(i,j) = 0;
% %         end
% %     end
% % end
% G = sparse(1:size(uc,1), 3*(uc(:,1)-1)+uc(:,2), ones(size(uc(:,3))));
% U = [Ktot, G'; G, sparse(size(uc,1), size(uc,1))] \ [F; uc(:,3)];
% %U = Ktot\F;
pset1 = find(node(:,2)<=0);                                % downNode
pset2 = find(node(:,1)<=0 & node(:,2)>0 & node(:,2)<1);    % leftNode
pset3 = find(node(:,2)>=1);                                % upNode
pset4 = find(node(:,1)>=1 & node(:,2)>0 & node(:,2)<1);    % rightNode
uc = [
    pset1, 1*ones(size(pset1)),zeros(size(pset1));
    pset1, 2*ones(size(pset1)),zeros(size(pset1));
    pset1, 3*ones(size(pset1)),zeros(size(pset1));
%     pset2, 2*ones(size(pset2)),zeros(size(pset2));
%     pset2, 3*ones(size(pset2)),zeros(size(pset2));
    pset3, 1*ones(size(pset3)),1*ones(size(pset3));
    pset3, 2*ones(size(pset3)),0*ones(size(pset3));
    pset3, 3*ones(size(pset3)),0*ones(size(pset3));
%     pset4, 3*ones(size(pset4)),0*ones(size(pset4));
    ];
% F = addPres(F, node, pset3,1e6,ndof,1);
%F((pset3-1)*ndof+2,1) = 1e6*1*1/numel(pset3);
for ii = 1:1:size(uc,1)
    i = ndof*(uc(ii,1)-1)+uc(ii,2);
    F(i,1) = uc(ii,3);
    for j = 1:1:size(Ktot,1)
        if(j~=i)
            F(j,1) = F(j,1) - Ktot(j,i)*uc(ii,3);
        end
    end
    Ktot(i,i)=1;
    for j = 1:1:size(Ktot,1)
        if(j~=i)
            Ktot(j,i) = 0; 
            Ktot(i,j) = 0;
        end
    end
end

U = Ktot\F;

figure(200)
ncoor = node + 0*[U(1:3:size(Ktot,1),1), U(2:3:size(Ktot,1),1)];
patch('Faces',element(:,[1,5,2,6,3,7,4,8]),'Vertices',ncoor,...
    'FaceVertexCData',U(1:3:size(Ktot,1),1),'FaceColor','interp',...%
    'edgecolor','none');
title('U_1')
colormap(jet(20))
colorbar
%axis off
axis equal

figure(300)
ncoor = node + 0*[U(1:3:size(Ktot,1),1), U(2:3:size(Ktot,1),1)];
patch('Faces',element(:,[1,5,2,6,3,7,4,8]),'Vertices',ncoor,...
    'FaceVertexCData',U(2:3:size(Ktot,1),1),'FaceColor','interp',...%
    'edgecolor','none');
title('U_2')
colormap(jet(20))
colorbar
%axis off
axis equal
figure(400)
ncoor = node + 0*[U(1:3:size(Ktot,1),1), U(2:3:size(Ktot,1),1)];
patch('Faces',element(:,[1,5,2,6,3,7,4,8]),'Vertices',ncoor,...
    'FaceVertexCData',U(3:3:end,1),'FaceColor','interp',...%
    'edgecolor','none');
colormap(jet(20))
colorbar
%axis off
axis equal


fprintf('max(U2)=%f\n', max(U(2:3:size(Ktot,1))));
fprintf('min(Omega3) = %f\n', min(U(3:3:size(Ktot,1))));
% 30x30
% max(U2)=0.139293
% min(Omega3) = -0.050547
% 50x50
% max(U2)=0.139298
% min(Omega3) = -0.050547
% stress calculation


