function homogenization_after_smooth_averaged()
resolution=25; 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;

load('voxel_afterSmooth25.mat', 'voxel')
nx=60;ny=1;nz=20;
DH=cell(nx,ny,nz);


ix=20;iy=1;iz=10;
for ix=1:nx
    for iy=1:ny
        for iz=1:nz
VOXEL_ele = voxel((iy-1)*resolution+1:iy*resolution,...
                  (ix-1)*resolution+1:ix*resolution,...        
                  (iz-1)*resolution+1:iz*resolution);

              
u = FEA_homo(resolution,VOXEL_ele,edofMat,Ke,Fe);
DH{ix,iy,iz} = Disp2DH(VOXEL_ele,u,edofMat,D0,intB,h);
% DH{ix,iy,iz} = max(DH{ix,iy,iz},D0.*1e-6);
        end
    end
end
save DH_afterSmooth25_averaged.mat DH
end

function DH = Disp2DH(voxel,u,edofMat,D0,intB,h)
%intB: 6x24,
% ue : nelex24
x_vec=voxel(:);
nele = size(edofMat,1);
DH=zeros(6,6);

I=eye(6);

for iele=1:nele
    edof = edofMat(iele,:);
    uie=u(edof,:);

    material=x_vec(iele);
    De=D0*material;
    tmp = (I*h^3-intB*uie);
    DH=DH+De*tmp;

end
% disp(DH2)
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