function Sensitivity()
resolution=10; h =1.0/resolution;
[D0,Ke,Fe] = LocalSMRT(resolution);
intB = GaussIntB(h);
[eleidx,mesh,VE]=periodicMesh(resolution);
nele=size(mesh,1);nnode = size(VE,1);
edofMat = zeros(nele,24);
edofMat(:,1:3:24) = mesh.*3-2;
edofMat(:,2:3:24) = mesh.*3-1;
edofMat(:,3:3:24) = mesh.*3;
ELEidx = zeros(nele,3);
for i=1:resolution
    for j=1:resolution
        for k=1:resolution
            ELEidx(eleidx(i,j,k),1)=i;
            ELEidx(eleidx(i,j,k),2)=j;
            ELEidx(eleidx(i,j,k),3)=k;
        end
    end
end
% a = 1.2;b = 0.8; c = 0.1;
a = 1; b = 1; c = 0;
num_c=40;
hc = (1+3)/num_c;
cc =  -3+hc/2:hc:1-hc/2;
dDHdc=cell(num_c,1); dVdc=zeros(num_c,1);
for ic=1:num_c
    disp([num2str(ic),'======================'])
    c=cc(ic);
voxel = ETPMSVoxel(resolution,a,b,c);
u = FEA_homo(resolution,voxel,edofMat,Ke,Fe);
% tic
[dV,DHdc] = Disp2DHdp(resolution,voxel,u,edofMat,D0,intB,h,ELEidx,Ke,a,b,c);
% toc
dDHdc{ic}=DHdc;
dVdc(ic)=dV;
end
save TPMS_CHdc_sample.mat
end

function [dV,DHdp] = Disp2DHdp(resolution,voxel,u,edofMat,D0,intB,h,ELEidx,Ke,a,b,c)
nGrid=25;
% dHdp = HeavisideGradient(resolution,nGrid,a,b,c);
% sum(dHdp,'all')

f = HeavisideGradient2(resolution,nGrid,a,b,c);
% sum(f,'all')
% sum(f,'all')/(resolution*nGrid)^3
dV=sum(f,'all')/(resolution*nGrid)^3;
Bs = BatGaussPoints(h,nGrid);

% x_vec=voxel(:);
nele = size(edofMat,1);
I=eye(6);
DHdp=zeros(6,6);
% for iele=1:nele
%     edof = edofMat(iele,:);
%     uie=u(edof,:);
% 
% %     material=x_vec(iele);
%     dh = dHdp(ELEidx(iele,1),ELEidx(iele,2),ELEidx(iele,3));
% %    material=voxel(ELEidx(iele,1),ELEidx(iele,2),ELEidx(iele,3));
% %     De=D0*material;
% %     tmp = (I*h^3-intB*uie);
% %     DH=DH+De*tmp;
% %     DHdp=DHdp+dh.*(De*(I*h^3-intB*uie)-uie.'*intB.'*De + material.*uie.'*Ke*uie);
%     DHdp=DHdp+dh.*(D0*(I*h^3-intB*uie)-uie.'*intB.'*D0 + uie.'*Ke*uie);
% end

for iele=1:nele
    edof = edofMat(iele,:);
    uie=u(edof,:);
    j = ELEidx(iele,1);i=ELEidx(iele,2);k=ELEidx(iele,3);
    dH = f((i-1)*nGrid+1:i*nGrid,(j-1)*nGrid+1:j*nGrid,(k-1)*nGrid+1:k*nGrid);
    for ix=1:nGrid
        for iy=1:nGrid
            for iz=1:nGrid
                B = Bs(:,:,ix,iy,iz);
%                 B = Bs(:,:,iy,ix,iz);
                DHdp = DHdp+dH(ix,iy,iz).*(D0*(I-B*uie)-uie.'*B.'*D0 + uie.'*Ke*uie);
            end
        end
    end
end
DHdp=DHdp.*h^3/nGrid^3;

end



function dVe = HeavisideGradient(resolution,nGrid,a,b,c)
h=1.0/(resolution*nGrid);
X = h/2:h:1-h/2;
[x,y,z]=meshgrid(X);
f=fun(x,y,z,a,b,c);

dVe = zeros(resolution,resolution,resolution);
for i = 1:resolution
    for j = 1:resolution
        for k = 1:resolution
%             dVe(i,j,k) = sum(sum(sum(f((i-1)*nGrid+1:i*nGrid,(j-1)*nGrid+1:j*nGrid,(k-1)*nGrid+1:k*nGrid))));
            dVe(j,i,k) = sum(sum(sum(f((i-1)*nGrid+1:i*nGrid,(j-1)*nGrid+1:j*nGrid,(k-1)*nGrid+1:k*nGrid))));
        end
    end
end
dVe = dVe./(resolution*nGrid)^3;
end



function u = FEA_homo(num,x,edofMat,Ke,Fe)
nele = num^3; nnode = num^3;
iK = reshape(kron(edofMat,ones(24,1))',24*24*nele,1);
jK = reshape(kron(edofMat,ones(1,24))',24*24*nele,1);
sK = reshape(Ke(:)*(x(:).'),24*24*nele,1);
K = sparse(iK,jK,sK); 

iK = reshape(kron(edofMat,ones(6,1))',24*6*nele,1);
jK = reshape(kron(repmat([1 2 3 4 5 6],[nele,1]),ones(1,24))',24*6*nele,1);
sK = reshape(Fe(:)*(x(:).'),24*6*nele,1);
F = sparse(iK,jK,sK);

% u = zeros(nnode*3,6);
% tic
% u(4:end,:)=K(4:end,4:end)\F(4:end,:);
% toc

u = zeros(nnode*3,6);
tic
for i=1:6
    u(4:end,i)=pcg(K(4:end,4:end),F(4:end,i),1e-4,500);
end
toc
end