function [dQ,dDHdx]= homogenization_test(x)
num = size(x,1);
nelx = num;
nely = num;
nelz = num;
% nelx = 20;
% nely = 20;
% nelz = 20; 
% num = 20;
nele = nelx*nely*nelz;

% x = rand(nelx,nely,nelz);
% x = ones(nely,nelx,nelz).*0.5;
% x(nely/2:nely/2+1,nelx/2:nelx/2+1,nelz /2:nelz/2+1) = 1e-9;
% x(nely/2-1:nely/2+2,nelx/2-1:nelx/2+2,nelz/2-1:nelz/2+2) = 1e-9;
% x = max(1e-9,x);
E0 = 1; nu = 0.3;
Emin = 1e-9;
penal = 3;
D0 = E0/(1+nu)/(1-2*nu)* [ 1-nu nu nu 0 0 0;
    nu 1-nu nu 0 0 0;
    nu nu 1-nu 0 0 0;
    0 0 0 (1-2*nu)/2 0 0;
    0 0 0 0 (1-2*nu)/2 0;
    0 0 0 0 0 (1-2*nu)/2;];
% disp(D0)
% [Q,dQ] = homogenization_energy(nelx,nely,nelz,D0,x,Emin,penal);
% disp(Q)
% disp(Q-D0*2)
[DH,dDHdx] = homogenization_my(num,D0,x,penal,E0,nu);
disp(DH)
% disp(Q-DH)
end

function [DH,dDHdx]=homogenization_my(num,D0,x,penal,E0,nu)
datafile = ['homKeFe_',num2str(num),'.mat'];
tic
if exist(datafile,'file')
    load(datafile,'Ke','Fe','intB','intDB')
else
    [Ke,Fe,intB,intDB] = symbolicKeFe(num,E0,nu);%========================================可预计算
    save(datafile,'Ke','Fe','intB','intDB')
end
toc
[eleidx,mesh,VE]=periodicMesh(num);
nele=size(mesh,1);nnode=nele;
edofMat = zeros(size(mesh,1),24);
edofMat(:,1:3:24) = mesh.*3-2;
edofMat(:,2:3:24) = mesh.*3-1;
edofMat(:,3:3:24) = mesh.*3;
iK = reshape(kron(edofMat,ones(24,1))',24*24*nele,1);
jK = reshape(kron(edofMat,ones(1,24))',24*24*nele,1);

iF = reshape(kron(edofMat,ones(6,1))',24*6*nele,1);
jF = reshape(kron(repmat([1 2 3 4 5 6],[nele,1]),ones(1,24))',24*6*nele,1);

sK = reshape(Ke(:)*((x(:).^penal).'),24*24*nele,1);
K = sparse(iK,jK,sK); %K = (K+K')/2;
sF = reshape(Fe(:)*((x(:).^penal).'),24*6*nele,1);
F = sparse(iF,jF,sF);
u = zeros(nnode*3,6);
% u(4:end,:)=K(4:end,4:end)\F(4:end,:);

for i=1:6
    u(4:end,i)=pcg(K(4:end,4:end),F(4:end,i),1e-6,300);
end

x_vec=x(:);
I=eye(6);h=1.0/num;
DH=zeros(6,6);
dDHdx = zeros(6,6,nele);
for iele=1:nele
    element=mesh(iele,:);
    edof=zeros(1,24);
    edof(1:3:24)=3*element(:)-2;
    edof(2:3:24)=3*element(:)-1;
    edof(3:3:24)=3*element(:);
    uie=u(edof,:);
    De = D0*(x_vec(iele).^penal);
%     DH=DH+De*(I*h^3-intB*uie);
    DH=DH+De*(I*h^3-intB*uie)+(x_vec(iele).^penal).*(uie.'*Ke*uie - uie.'*Fe);
    dDHdx(:,:,iele) = penal*x(iele)^(penal-1).*(D0.*h^3 - 2.*intDB*uie + uie.'*Ke*uie);
end
end

function [Q,dQ] = homogenization_energy(nelx,nely,nelz,D0,xPhys,Emin,penal)
Emin=0;
nele = nelx*nely*nelz;
% KE = elementMatVec3D(1.0/nelx, 1.0/nely, 1.0/nelz, D0);
KE = elementMatVec3D(1.0/2, 1.0/2, 1.0/2, D0);
Num_node = (1+nely)*(1+nelx)*(1+nelz);
nodenrs = reshape(1:Num_node,1+nely,1+nelx,1+nelz);
edofVec = reshape(3*nodenrs(1:end-1,1:end-1,1:end-1)+1,nelx*nely*nelz,1);
edofMat = repmat(edofVec,1,24)+repmat([0 1 2 3*nely+[3 4 5 0 1 2] -3 -2 -1 3*(nelx+1)*(nely+1)+[0 1 2 3*nely+[3 4 5 0 1 2] -3 -2 -1]], nele, 1);
iK = reshape(kron(edofMat,ones(24,1))',24*24*nele,1);
jK = reshape(kron(edofMat,ones(1,24))',24*24*nele,1);
n1 = [nodenrs(end, [1 end], 1) nodenrs(1, [end 1], 1) nodenrs(end, [1 end], end) nodenrs(1, [end 1], end)];
d1 = reshape([3*n1-2; 3*n1-1; 3*n1],3*numel(n1),1);
n3 = [reshape(squeeze(nodenrs(end,1,2:end-1)),1,numel(squeeze(nodenrs(end,1,2:end-1))))...
    reshape(squeeze(nodenrs(1, 1, 2:end-1)),1,numel(squeeze(nodenrs(1, 1, 2:end-1))))...
    reshape(squeeze(nodenrs(end,2:end-1,1)),1,numel(squeeze(nodenrs(end,2:end-1,1))))...
    reshape(squeeze(nodenrs(1, 2:end-1, 1)),1,numel(squeeze(nodenrs(1, 2:end-1, 1))))...
    reshape(squeeze(nodenrs(2:end-1, 1, 1)),1,numel(squeeze(nodenrs(2:end-1, 1, 1))))...
    reshape(squeeze(nodenrs(2:end-1,1,end)),1,numel(squeeze(nodenrs(2:end-1,1,end))))...
    reshape(squeeze(nodenrs(2:end-1, 2:end-1, 1)),1,numel(squeeze(nodenrs(2:end-1, 2:end-1, 1))))...
    reshape(squeeze(nodenrs(2:end-1, 1, 2:end-1)),1,numel(squeeze(nodenrs(2:end-1, 1, 2:end-1))))...
    reshape(squeeze(nodenrs(end,2:end-1,2:end-1)),1,numel(squeeze(nodenrs(end,2:end-1,2:end-1))))];
d3 = reshape([3*n3-2; 3*n3-1; 3*n3],3*numel(n3),1);
n4 = [reshape(squeeze(nodenrs(1, end, 2:end-1)),1,numel(squeeze(nodenrs(1, end, 2:end-1))))...
    reshape(squeeze(nodenrs(end,end,2:end-1)),1,numel(squeeze(nodenrs(end,end,2:end-1))))...
    reshape(squeeze(nodenrs(1, 2:end-1, end)),1,numel(squeeze(nodenrs(1, 2:end-1, end))))...
    reshape(squeeze(nodenrs(end,2:end-1,end)),1,numel(squeeze(nodenrs(end,2:end-1,end))))...
    reshape(squeeze(nodenrs(2:end-1,end,end)),1,numel(squeeze(nodenrs(2:end-1,end,end))))...
    reshape(squeeze(nodenrs(2:end-1, end, 1)),1,numel(squeeze(nodenrs(2:end-1, end, 1))))...
    reshape(squeeze(nodenrs(2:end-1,2:end-1,end)),1,numel(squeeze(nodenrs(2:end-1,2:end-1,end))))...
    reshape(squeeze(nodenrs(2:end-1,end,2:end-1)),1,numel(squeeze(nodenrs(2:end-1,end,2:end-1))))...
    reshape(squeeze(nodenrs(1, 2:end-1, 2:end-1)),1,numel(squeeze(nodenrs(1, 2:end-1, 2:end-1))))];
d4 = reshape([3*n4-2; 3*n4-1; 3*n4],3*numel(n4),1);
n2 = setdiff(nodenrs(:),[n1(:);n3(:);n4(:)]);
d2 = reshape([3*n2-2; 3*n2- 1; 3*n2],3*numel(n2),1);
e = eye(6); ufixed = zeros(24,6);
vert_cor = [0, nelx, nelx, 0, 0, nelx, nelx, 0;
    0, 0, nely, nely, 0, 0, nely, nely;
    0, 0, 0, 0, nelz, nelz, nelz, nelz];
for i = 1:6
    epsilon = [ e(i,1), e(i,4)/2, e(i,6)/2;
        e(i,4)/2, e(i,2), e(i,5)/2;
        e(i,6)/2, e(i,5)/2, e(i,3)];
    ufixed(:,i) = reshape(epsilon*vert_cor,24,1);
end
% 3D boundary constraint equations
wfixed = [repmat(ufixed(7:9,:),numel(squeeze(nodenrs(end,1,2:end-1))),1);
    repmat(ufixed( 4:6,:)-ufixed(10:12,:),numel(squeeze(nodenrs(1, 1, 2:end-1))),1);
    repmat(ufixed(22:24,:),numel(squeeze(nodenrs(end,2:end-1,1))),1);
    repmat(ufixed(13:15,:)-ufixed(10:12,:),numel(squeeze(nodenrs(1, 2:end-1, 1))),1);
    repmat(ufixed(16:18,:),numel(squeeze(nodenrs(2:end-1, 1, 1))),1);
    repmat(ufixed( 4:6,:)-ufixed(13:15,:),numel(squeeze(nodenrs(2:end-1,1,end))),1);
    repmat(ufixed(13:15,:),numel(squeeze(nodenrs(2:end-1, 2:end-1,1))),1);
    repmat(ufixed(4:6,:),numel(squeeze(nodenrs(2:end-1, 1, 2:end- 1))),1);
    repmat(ufixed(10:12,:),numel(squeeze(nodenrs(end,2:end-1,2:end-1))),1)];
% sK = reshape(KE(:)*(Emin+xPhys(:)'.^penal*(1-Emin)),24*24*nele,1);
sK = reshape(KE(:)*(Emin+xPhys(:)'.^penal*(1-Emin)),24*24*nele,1);
K = sparse(iK,jK,sK); K = (K+K')/2;
Kr = [K(d2,d2), K(d2,d3)+K(d2,d4);
    K(d3,d2)+K(d4,d2),K(d3,d3)+K(d4,d3)+K(d3,d4)+K(d4,d4)];
U(d1,:) = ufixed;
U([d2;d3],:) = Kr\(-[K(d2,d1);K(d3,d1)+K(d4,d1)]*ufixed-[K(d2,d4);K(d3,d4)+K(d4,d4)]*wfixed);
U(d4,:) = U(d3,:)+wfixed;
cellVolume = nele;
% cellVolume = 1;
qe = cell(6,6);
Q  = zeros(6,6);dQ = cell(6,6);
for i = 1:6
    for j = 1:6
        U1 = U(:,i);
        U2 = U(:,j);
        qe{i,j} = reshape(sum((U1(edofMat)*KE).*U2(edofMat),2),nely,nelx,nelz);
        Q(i,j) = 1/cellVolume*sum(sum(sum((Emin+xPhys.^penal*(1-Emin)).*qe{i,j})));
        dQ{i,j} = 1/cellVolume*(penal*(1-Emin)*xPhys.^(penal-1).*qe{i,j});
    end
end
end

function Ke = elementMatVec3D(a, b, c, DH)
GN_x=[-1/sqrt(3),1/sqrt(3)];
GN_y=GN_x;
GN_z=GN_x;
GaussWeigh=[1,1];
Ke = zeros(24,24); L = zeros(6,9);
L(1,1) = 1; L(2,5) = 1; L(3,9) = 1;
L(4,2) = 1; L(4,4) = 1; L(5,6) = 1;
L(5,8) = 1; L(6,3) = 1; L(6,7) = 1;
for i=1:length(GN_x)
    for j=1:length(GN_y)
        for k=1:length(GN_z)
            x = GN_x(i);y = GN_y(j);z = GN_z(k);
            dNx = 1/8*[-(1-y)*(1-z), (1-y)*(1-z), (1+y)*(1-z), -(1+y)*(1-z), -(1-y)*(1+z), (1-y)*(1+z), (1+y)*(1+z) -(1+y)*(1+z)];
            dNy = 1/8*[-(1-x)*(1-z) -(1+x)*(1-z), (1+x)*(1-z), (1-x)*(1-z), -(1-x)*(1+z), -(1+x)*(1+z), (1+x)*(1+z), (1-x)*(1+z)];
            dNz = 1/8*[-(1-x)*(1-y) -(1+x)*(1-y) -(1+x)*(1+y) -(1-x)*(1+y), (1-x)*(1-y), (1+x)*(1-y), (1+x)*(1+y), (1-x)*(1+y)];
            J = [dNx;dNy;dNz]*[ -a a a -a -a a a -a ;
                -b -b b b -b -b b b;
                -c -c -c -c c c c c]';
            G = [inv(J) zeros(3) zeros(3);zeros(3) inv(J) zeros(3);zeros(3) zeros(3) inv(J)];
            dN(1,1:3:24) = dNx; dN(2,1:3:24) = dNy; dN(3,1:3:24) = dNz;
            dN(4,2:3:24) = dNx; dN(5,2:3:24) = dNy; dN(6,2:3:24) = dNz;
            dN(7,3:3:24) = dNx; dN(8,3:3:24) = dNy; dN(9,3:3:24) = dNz;
            Be = L*G*dN;
            Ke = Ke + GaussWeigh(i)*GaussWeigh(j)*GaussWeigh(k)*det(J)*(Be'*DH*Be);
        end
    end
end
end

function [eleidx,mesh,VE]=periodicMesh(num)
% [0,1]^3 
nele=num^3;
nnod=(num+1)^3;
nodenrs=reshape(1:nnod,1+num,1+num,1+num);
edofvec=reshape(nodenrs(1:end-1,1:end-1,1:end-1),nele,1);
edofMat=repmat(edofvec,1,8)+repmat([0 1 num+1 num+2 (num+1)^2+[0 1 num+1 num+2]],nele,1);

nnp=nele;%total number of unique nodes
nnpArray=reshape(1:nnp,num,num,num);
eleidx=nnpArray;%element indices

nnpArray(end+1,:,:)=nnpArray(1,:,:);
nnpArray(:,end+1,:)=nnpArray(:,1,:);
nnpArray(:,:,end+1)=nnpArray(:,:,1);

dofvector=zeros(nnod,1);
dofvector(:)=nnpArray(:);
% dofvector(2:3:end)=3*nnpArray(:)-1;
% dofvector(3:3:end)=3*nnpArray(:);

mesh=dofvector(edofMat);

% coordinates of the 1st node of each element
h=1.0/num;
%V=zeros(num,num,num,3);
VE=zeros(nele,3);
for i=1:num
    for j=1:num
        for k=1:num
            %V(i,j,k,:)=[(i-1)*h,(j-1)*h,(k-1)*h];
            VE(eleidx(i,j,k),:)=[(i-1)*h,(j-1)*h,(k-1)*h];
        end
    end
end
end

function [Ke,Fe,intB,intDB] = symbolicKeFe(num,E,m)
h=1.0/num;
syms x y z
x0=0; y0=0; z0=0;
N(1) = (h+x0-x) * (h+y0-y) * (h+z0-z) / (h^3);
N(2) = (x-x0) * (h+y0-y) * (h+z0-z) / (h^3);
N(3) = (h+x0-x) * (y-y0) * (h+z0-z) / (h^3);
N(4) = (x-x0) * (y-y0) * (h+z0-z) / (h^3);
N(5) = (h+x0-x) * (h+y0-y) * (z-z0) / (h^3);
N(6) = (x-x0) * (h+y0-y) * (z-z0) / (h^3);
N(7) = (h+x0-x) * (y-y0) * (z-z0) / (h^3);
N(8) = (x-x0) * (y-y0) * (z-z0) / (h^3);

for j=1:8
    for jj=1:3
        Dif(3*jj-2,j*3+jj-3) = diff(N(j),x);
        Dif(3*jj-1,j*3+jj-3) = diff(N(j),y);
        Dif(3*jj,  j*3+jj-3) = diff(N(j),z);
    end
end
B9x24 = Dif;
for j=1:24
    B(1,j) = B9x24(1,j);
    B(2,j) = B9x24(5,j);
    B(3,j) = B9x24(9,j);
    B(4,j) = B9x24(2,j) + B9x24(4,j);
    B(5,j) = B9x24(6,j) + B9x24(8,j);
    B(6,j) = B9x24(3,j) + B9x24(7,j);
end
% Young's modulus & Poisson's ratio
%E = 2e11; m = 0.33;
a = E / ( (1+m) * (1-2*m) );
% 6 X 6 空间问题的弹性系数矩阵
D = [a*(1-m) a*m     a*m     0         0         0
    a*m      a*(1-m) a*m     0         0         0
    a*m      a*m     a*(1-m) 0         0         0
    0        0       0       a*(0.5-m) 0         0
    0        0       0       0         a*(0.5-m) 0
    0        0       0       0         0         a*(0.5-m)];

K=B.'*D*B;
Ke=zeros(24,24);
for i=1:24
    for j=i:24
        Ke(i,j)=int(int(int(K(i,j),'x',0,h),'y',0,h),'z',0,h);
    end
end
Ke=Ke+Ke.'-diag(diag(Ke));

F=B.'*D;
Fe=zeros(24,6);
for i=1:24
    for j=1:6
        Fe(i,j)=int(int(int(F(i,j),'x',0,h),'y',0,h),'z',0,h);
    end
end
intB=zeros(6,24);
for i=1:6
    for j=1:24
        intB(i,j)=double(int(int(int(B(i,j),'x',0,h),'y',0,h),'z',0,h));
    end
end

intDB=zeros(6,24);
DB = D*B;
for i=1:6
    for j=1:24
        intDB(i,j)=double(int(int(int(DB(i,j),'x',0,h),'y',0,h),'z',0,h));
    end
end

end