function Sample_DHandDHdp()
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;

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;b = 1.2; c = 0.1;
% a = 1.0;b = 1.0; c = 0.0;
% num_c=20;
% hc = (1+3)/num_c;
% cc =  -3+hc/2:hc:1-hc/2;
% CH=cell(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);
% % u=rand(nnode*3,1);
% % 
% % tmp = u(edofMat);
% % size(tmp)
% % tmp(1234,:) - u(edofMat(1234,:)).'
% % DH = Disp2DH(voxel,u,D0,intB,h);
% % tic
% CH{ic} = Disp2DH(voxel,u,edofMat,D0,intB,h,ELEidx,Ke);
% % toc
% end
% save ETPMS_CHsample.mat

% a = 1;b = 1.2; c = 0.1;
% a = 1.0;b = 1.2; c = 0.5;
% num_a=15;
% ha = (2-0.5)/num_a;
% aa = 0.5+ha/2:ha:2-ha/2;
% CH=cell(num_a,1);
% for ia=1:num_a
%     disp([num2str(ia),'======================'])
%     a=aa(ia);
%     if 1+a+b+c<=0
%         CH{ia} = D0;
%     else
%         voxel = ETPMSVoxel(resolution,a,b,c);
%         u = FEA_homo(resolution,voxel,edofMat,Ke,Fe);
%         CH{ia} = Disp2DH(voxel,u,edofMat,D0,intB,h,ELEidx,Ke);
%         % Ve=ETPMS_volume(resolution,a,b,c);
%         % CH{ia} = Disp2DH2(voxel,u,edofMat,D0,intB,h,Ve,ELEidx);
%     end
% end
% save ETPMS_CHsample_a.mat

num_a = 15;num_b=15;num_c=60;
ha = (2-0.5)/num_a;hb = (2-0.5)/num_b;hc = (1+5)/num_c;
aa = 0.5+ha/2:ha:2-ha/2;
bb = 0.5+hb/2:hb:2-hb/2;
cc =  -5+hc/2:hc:1-hc/2;
CH=cell(num_a,num_b,num_c);
% for ia=1:num_a
for ia=6:10%1:5%num_a
    a=aa(ia);
    for ib=1:num_b
        b=bb(ib);
        for ic=1:num_c
            disp(['========================',num2str(ia),',',num2str(ib),',',num2str(ic)])
            c=cc(ic);
            if 1+a+b+c<=0
                CH{ia,ib,ic} = D0;
            else
                voxel = ETPMSVoxel(resolution,a,b,c);
                u = FEA_homo(resolution,voxel,edofMat,Ke,Fe);
                CH{ia,ib,ic} = Disp2DH(voxel,u,edofMat,D0,intB,h,ELEidx,Ke);
                % Ve=ETPMS_volume(resolution,a,b,c);
                % CH{ia} = Disp2DH2(voxel,u,edofMat,D0,intB,h,Ve,ELEidx);
            end
        end
    end
    save ETPMS_CHsample_abc_6_10.mat
end
end

function DH = Disp2DH2(voxel,u,edofMat,D0,intB,h,Ve,ELEidx)
%intB: 6x24,
% ue : nelex24
% x_vec=voxel(:);
x_vec=Ve(:);
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);
    material=Ve(ELEidx(iele,1),ELEidx(iele,2),ELEidx(iele,3));
    De=D0*material;
    tmp = (I*h^3-intB*uie);
    DH=DH+De*tmp;
%     DH2=DH2+De*(I*h^3-intB*uie)-uie.'*intB.'*De + material.*uie.'*Ke*uie;
%     DH2=DH2 - uie(:,1).'*intB.'*De(:,1) + material.*uie.'*Ke*uie;
end
end

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

% for i=1:6
%     Ii=I(:,i);
%     Di=zeros(6,1);
%     for iele=1:nele
%         edof = edofMat(iele,:);
%         uie=u(edof,i);
%         
%         material=x_vec(iele);
% %         material=voxel(ELEidx(iele,1),ELEidx(iele,2),ELEidx(iele,3));
%         De=D0*material;
%         Di=Di+De*(Ii*h^3-intB*uie);
%     end
%     DH(:,i)=Di;
% end
I=eye(6);
DH2=zeros(6,6);
% DH2=0;
for iele=1:nele
    edof = edofMat(iele,:);
    uie=u(edof,:);

    material=x_vec(iele);
%         material=voxel(ELEidx(iele,1),ELEidx(iele,2),ELEidx(iele,3));
    De=D0*material;
    tmp = (I*h^3-intB*uie);
    DH=DH+De*tmp;
%     DH2=DH2+De*(I*h^3-intB*uie)-uie.'*intB.'*De + material.*uie.'*Ke*uie;
%     DH2=DH2 - uie(:,1).'*intB.'*De(:,1) + material.*uie.'*Ke*uie;
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