%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2022-04-30 14:52:50
 % @LastEditTime: 2022-05-12 11:26:22
%--------------------------
dbstop if error
feature accel on
addpath util util_simulation util_bezier util_micro util_mtimesx util_output
addpath util_cvt mexCVT util_tetmesh util_MMC util_intersection

%% init
vF = 0.001;
optDesign = 'cantilever';

volfrac = 0.3;
datapath = "data/mat/cvt_cmpPBC_"+optDesign+"_v"+volfrac+".mat";
load(datapath);

nelx = 8;
nely = 2; 
nelz = 2;
microx = 16; % 16;

lx = 1;
dx = lx / microx;

globalx = nelx * microx;
globaly = nely * microx;
globalz = nelz * microx;

halfx = nelx * lx / 2;
halfy = nely * lx / 2;
halfz = nelz * lx / 2;
maxx = halfx; minx = -halfx;
maxy = halfy; miny = -halfy;
maxz = halfz; minz = -halfz;

e1 = 1; nu0 = 0.3; e0 = 1e-5; penal = 3;

optChanged = 1;

num_add = 0; % additional macro dofs on the border

% global
[u1, l1, D1] = material_paras(e1, nu0);
[iK_global, jK_global, edofMat_global] = prepare_assemble(globalx, globaly, globalz);

opt_scale = 1;
ref_nodes_global = mesh3D(nelx, nely, nelz, microx, 1, lx, opt_scale);
ref_nodes_global(:, 1) = ref_nodes_global(:, 1) - halfx;
ref_nodes_global(:, 2) = ref_nodes_global(:, 2) - halfy;
ref_nodes_global(:, 3) = ref_nodes_global(:, 3) - halfz;
[freedofs_global, ~, fext_global, ~] = design_domain(optDesign, maxx, minx, maxy, miny, maxz, minz, vF, ref_nodes_global, optChanged);

% macro FEA
opt_scale = 2;

num_p = 2; % 2 control pnts for every segment
num_b = (num_p + 1) * (num_add + 1) - 1; % added nodes on one edge, including bridge nodes & bezier pnts

ref_nodes = mesh3D(nelx, nely, nelz, microx, num_b, lx, opt_scale);
ref_nodes(:, 1) = ref_nodes(:, 1) - halfx;
ref_nodes(:, 2) = ref_nodes(:, 2) - halfy;
ref_nodes(:, 3) = ref_nodes(:, 3) - halfz;
[freedofs, ~, fext, alldofs] = design_domain(optDesign, maxx, minx, maxy, miny, maxz, minz, vF, ref_nodes, optChanged);
U = zeros(alldofs, 1);

% figure; scatter3(ref_nodes(:,1), ref_nodes(:,2), ref_nodes(:,3));

[iK_ma, jK_ma, edofMat_ma] = assemble_bezier(nelx, nely, nelz, num_b);

c_polyMesh = prep_polyMesh(edofMat_ma, ref_nodes);

% micro FEA
nele_mi = microx^3;

ke1 = intKE(dx/2, dx/2, dx/2, D1);
[iK_mi, jK_mi, edofMat_mi] = prepare_assemble(microx, microx, microx);

[boundarys, inner] = micro_boundary(microx);
newdofs = [boundarys(:); inner(:)];
dofid   = reorder_dofs(newdofs); 
sort_u  = prepare_index_micro_u(microx, edofMat_mi, dofid);
bdofs_num = length(boundarys);

bezier_B = parametric_B(microx, num_add, num_b);

c_nodesFine = gen_fine_mesh(microx, nelx, nely, nelz, lx, minx, maxx, miny, maxy, minz, maxz);

npnts   = size(vars_loop, 1);

% homogenization
[wfixed, ufixed, d1, d2, d3, d4] = PBC(microx, microx, microx, dx);

[iK_PBC, jK_PBC, edofMat_PBC] = prepare_assemble(nelx, nely, nelz);

opt_scale = 1;
ref_nodes = mesh3D(nelx, nely, nelz, 1, 1, lx, opt_scale);
ref_nodes(:, 1) = ref_nodes(:, 1) - halfx;
ref_nodes(:, 2) = ref_nodes(:, 2) - halfy;
ref_nodes(:, 3) = ref_nodes(:, 3) - halfz;
[freedofs_PBC, ~, fext_PBC, ~] = design_domain(optDesign, maxx, minx, maxy, miny, maxz, minz, vF, ref_nodes, optChanged);

%% comparison
maxloop = size(vars_loop, 3);

comp_loop = zeros(maxloop, 3);
error_loop = zeros(maxloop, 2);

datapath = "data/mat/error_"+optDesign+"_v"+volfrac+".mat";

for loop = 1 : maxloop

    seeds = vars_loop(:,1:3,loop);
    T     = vars_loop(:,4,loop);

    indices = 1 : int32(npnts);
    [c_edges, cenP, vedges, c_nnbEdgeIDX, c_nnbSeedIDX] = intersection_info(seeds, ...
        indices, nelx, nely, nelz, lx, optDesign);   

    c_interPolySeed = do_intersection(vedges, c_polyMesh, c_nnbEdgeIDX, seeds);

    % MMC
    MMCs = update_MMCs(vedges, T, c_nnbSeedIDX);

    [c_rho, c_phi] = projection_hex(c_nodesFine, edofMat_mi, MMCs, ...
        dx*4, c_nnbEdgeIDX, c_interPolySeed);
    
    %% CBN
    % c_CBN_hex = simulation_CBN(c_rho, bezier_B, dofid, edofMat_mi, microx, e0, e1, penal, ke1, ...
        % iK_mi, jK_mi, iK_ma, jK_ma, freedofs, fext, bdofs_num);
    
    c_CBN_hex = obj_loop(loop, 1);
    
    %% full-scale
    [c_global, U_global] = simulation_fullscale(iK_global, jK_global, freedofs_global, fext_global, ...
        c_rho, e0, e1, penal, nelx, nely, nelz, microx, ke1);
    
%     def_nodes = reshape(U_global, 3, [])' + ref_nodes_global;
%     def_nodes(def_nodes(:,2) < -4, 2) = -3.86;
%     write_vtk(def_nodes, edofMat_global);

    %% homogenization
    [c_PBC, U_PBC] = simulation_homo(c_rho, iK_PBC, jK_PBC, fext_PBC, freedofs_PBC, e0, e1, penal, ke1, ...
        d1, d2, d3, d4, ufixed, wfixed, microx, edofMat_mi, lx, iK_mi, jK_mi);
    
%     U_PBC = U_PBC / 2;
%     recover_def_PBC(U_PBC, edofMat_PBC, ref_nodes, c_nodesFine, 50, MMCs);
    
    %% comparison
    PBC_err = (c_PBC-c_global)^2 / c_global^2;
    cbn_hex_err = (c_CBN_hex-c_global)^2 / c_global^2;
    % cbn_vor_err = (c_CBN_vor-c_global)^2 / c_global^2;
    
    comp_loop(loop, :) = [c_CBN_hex, c_PBC, c_global];
    error_loop(loop, :) = [cbn_hex_err, PBC_err];
end

save(datapath, 'comp_loop', 'error_loop');

%% utils
function c = simulation_CBN(c_rho, bezier_B, dofid, edofMat_mi, microx, e0, e1, penal, ke0, ...
    iK_mi, jK_mi, iK_ma, jK_ma, freedofs, F, bdofs_num)

    nele = size(c_rho, 1);
    order1 = reshape(edofMat_mi', [], 1);

    new_iK = dofid(iK_mi);
    new_jK = dofid(jK_mi);

%     c_psi = cell(nele,1);
    nvar = size(bezier_B, 2);
    sK_ma = zeros(nvar^2, nele);

    parfor ele = 1 : nele
        rho = c_rho{ele};        
        sK1  = reshape(ke0(:) * (e0 + rho(:)'.^penal * (e1 - e0)), [], 1);
        K = sparse(new_iK, new_jK, sK1(:)); 
        K = (K + K') / 2;

        M = assembleM2(bdofs_num, K, bezier_B); % [i,v] = M * B
        
        R = [bezier_B; M]; % [ndofs, v]                
        R1 = R(dofid(order1), :); % [24*nele, v]
        R2 = reshape(R1, 24, microx^3, []); % [24, m, v]

        psi1 = permute(R2, [1,3,2]);   % [24, v, m]        
        psi2 = permute(psi1, [2,1,3]); % [v,24, m]

        sK2 = reshape(sK1, 24, 24, []); % [12,12,m]
        Ke0 = mtimesx(mtimesx(psi2, sK2), psi1); % [3v,3v,m]
        Ke1 = squeeze(sum(Ke0, 3)); % [3v,3v], each ele is different

        sK_ma(:, ele) = Ke1(:);
    end
    K = sparse(iK_ma, jK_ma, sK_ma(:));
    K = (K + K')/2;    

%     nvar_global = size(K, 1);
%     S = spdiags(repmat(1e-5, nvar_global, 1),0,nvar_global,nvar_global);
%     K = K + S;

    U = zeros(size(F));
    U(freedofs,:) = K(freedofs,freedofs)\F(freedofs,:);

    c = F' * U;
end

function [c, U] = simulation_homo(c_rho, iK_ma, jK_ma, F, freedofs, e0, e1, penal, ke0, ...
    d1, d2, d3, d4, ufixed, wfixed, microx, edofMat_mi, lx, iK_mi, jK_mi)

    nele = size(c_rho, 1);
    sK_ma = zeros(24^2, nele);
    vol_mi = lx^3;

    parfor ele = 1 : nele
        rho = c_rho{ele};       
        rho1 = (e0 + rho(:)'.^penal * (e1 - e0)); 
        
        sK1  = reshape(ke0(:) * rho1, [], 1);
        K = sparse(iK_mi, jK_mi, sK1(:)); 
        K = (K + K') / 2;

        Kr = [K(d2, d2), K(d2, d3) + K(d2, d4);
        K(d3, d2) + K(d4, d2), K(d3, d3) + K(d3, d4) + K(d4, d3) + K(d4, d4)];
    
        Ue = zeros((microx+1)^3, 6);
        Ue(d1, :) = ufixed;
        Ue([d2; d3], :) = Kr \ (-[K(d2, d1); K(d3, d1) + K(d4, d1)] * ufixed - ...
            [K(d2, d4); K(d3, d4) + K(d4, d4)] * wfixed);
        Ue(d4, :) = Ue(d3, :) + wfixed;    

        % homog
        qe = cell(6, 6);
        Q = zeros(6, 6);
        for i = 1:6
            for j = 1:6            
                U1 = Ue(:,i);
                U2 = Ue(:,j);
                qe{i,j} = sum((U1(edofMat_mi)*ke0).*U2(edofMat_mi),2);
                Q(i,j) = 1/vol_mi * sum(rho1(:) .* qe{i,j}(:));
            end
        end        
        KE = intKE(lx/2, lx/2, lx/2, Q);
        sK_ma(:, ele) = KE(:);
    end
    K = sparse(iK_ma, jK_ma, sK_ma(:));
    K = (K + K')/2;    

    U = zeros(size(F));
    U(freedofs,:) = K(freedofs,freedofs)\F(freedofs,:);

    c = F' * U;
end


function write_vtk(ref_nodes, edofMat)
    ref_nodes_new = ref_nodes;
    ref_nodes_new(:, 4) = 1:size(ref_nodes, 1);
    eles = zeros(size(edofMat, 1), 8);
    for ele = 1:size(edofMat, 1)
        edof = edofMat(ele, :);
        nid0 = edof(3:3:end) / 3;

        [nid, ~] = find(ref_nodes_new(:,4)==nid0);
        eles(ele, :) = nid;
    end
    dataPath = ['data/vtk/reference.vtk'];
    voxel2vtk(dataPath, ref_nodes_new, eles);
end


function recover_def_PBC(U, edofMat, ref_nodes, c_nodesFine, microx, MMCs)

    ref = reshape(ref_nodes', [], 1);

    for ele = 1:size(edofMat, 1)
        nodes = c_nodesFine{ele};
        [regular_ref, regular_phi] = regular_ref_nodes(nodes, MMCs, microx);
        cx = reshape(regular_ref(:,1), microx, microx, microx);
        cy = reshape(regular_ref(:,2), microx, microx, microx);
        cz = reshape(regular_ref(:,3), microx, microx, microx);
        regular_phi = reshape(regular_phi, microx, microx, microx);
        [faces, verts] = isosurface(cx,cy,cz,regular_phi,0);
        % phi = reshape(c_phi{ele}, microx+1, microx+1, microx+1);
        % cx = reshape(nodes(:,1), microx+1, microx+1, microx+1);
        % cy = reshape(nodes(:,2), microx+1, microx+1, microx+1);
        % cz = reshape(nodes(:,3), microx+1, microx+1, microx+1);
        % [faces, verts] = isosurface(cx,cy,cz,phi,0);

        edof = edofMat(ele, :);
        ue = U(edof);        
        re = reshape(ref(edof), 3, [])';
        de = reshape(ue, 3, [])' + re;

        Fx = scatteredInterpolant(re(:,1),re(:,2),re(:,3),de(:,1),'natural');
        Fy = scatteredInterpolant(re(:,1),re(:,2),re(:,3),de(:,2),'natural');
        Fz = scatteredInterpolant(re(:,1),re(:,2),re(:,3),de(:,3),'natural');

        vqx = Fx(verts); 
        vqy = Fy(verts);
        vqz = Fz(verts);
        stlwrite(['data/stl/output1-pbc',num2str(ele),'.stl'], faces, [vqx,vqy,vqz]);

        [faces, verts] = isocaps(cx,cy,cz,regular_phi,0);
        vqx = Fx(verts);
        vqy = Fy(verts);
        vqz = Fz(verts);
        stlwrite(['data/stl/output2-pbc',num2str(ele),'.stl'], faces, [vqx,vqy,vqz]);
    end
end

function [regular_ref, regular_phi] = regular_ref_nodes(ref, MMCs, microx)
    minx = min(ref(:,1)); maxx = max(ref(:,1));
    miny = min(ref(:,2)); maxy = max(ref(:,2));
    minz = min(ref(:,3)); maxz = max(ref(:,3));

    [gx,gy,gz] = meshgrid(linspace(minx, maxx, microx), linspace(miny, maxy, microx), linspace(minz, maxz, microx));
    regular_ref = [gx(:), gy(:), gz(:)];    

    regular_phi = repmat(-1e5, size(regular_ref,1), 1);

    parfor i = 1 : size(MMCs, 1)
        t  = MMCs(i, 1);
        sp = MMCs(i, 2:4);
        ep = MMCs(i, 5:7);

        phi0 = signed_distance(regular_ref, sp, ep, t);
        regular_phi = max(phi0, regular_phi);
    end
end

function [c, U] = simulation_fullscale(iK, jK, freedofs, F, c_rho, e0, e1, penal, nelx, nely, nelz, microx, KE)

    globalx = nelx * microx;
    globaly = nely * microx;
    globalz = nelz * microx;
    global_rho = zeros(globaly, globalx, globalz);

    for i = 0:nelx-1
        for j = 0:nely-1
            for k = 0:nelz-1
                ele = j+1 + i*nely + k*nelx*nely;
                rho = reshape(c_rho{ele}, microx, microx, microx);
                                
                global_rho(j*microx+1:(j+1)*microx, i*microx+1:(i+1)*microx, k*microx+1:(k+1)*microx) = rho;
            end
        end
    end
    
    nele = globalx * globaly * globalz;
    sK = reshape(KE(:) * (e0 + global_rho(:)'.^penal*(e1-e0)),24*24*nele,1);
    K = sparse(iK,jK,sK); K = (K+K')/2;

    U = zeros(size(F));
    U(freedofs,:) = K(freedofs,freedofs)\F(freedofs,:);

    c = F' * U;
end

function [wfixed, ufixed, d1, d2, d3, d4] = PBC(nelx, nely, nelz, dx)
    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);
    
    % periodic boundary conditions
    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);

    % the imposing of six linearly independent unit test strains
    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] * dx;
    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)];
end


function c_nodes = gen_fine_mesh(microx, nelx, nely, nelz, lx, minx, maxx, miny, maxy, minz, maxz)

    nele = nelx * nely * nelz;
    c_nodes = cell(nele, 1);

    dx = lx / microx;

    [gx, gy, gz] = meshgrid(0:dx:lx, 0:dx:lx, 0:dx:lx);
    gy = flipud(gy);
    nodes0 = [gx(:), gy(:), gz(:)];
    
%     figure;
%         xlabel('x axis');
%     ylabel('y axis');
%     zlabel('z axis');

    for i = 0 : nelx-1
        for j = 0 : nely-1
            for k = 0 : nelz-1
                step = [lx * i + minx, lx * (nely - j) - lx - maxy, lx * k + minz];
                nodes = nodes0 + repmat(step, size(nodes0, 1), 1);
                
%                 scatter3(nodes(:,1), nodes(:,2), nodes(:,3)); hold on;

                ele = j+1 + i*nely + k*nelx*nely;
                c_nodes{ele} = nodes;
            end
        end
    end
end

function [c_rho, c_phi] = projection_hex(c_nodes, edofMat_mi, MMCs, epsilon, c_nnbEdgeIDX, c_interPolySeed)

    ncvt  = size(c_nodes, 1);
    npnts = size(c_interPolySeed, 1);
    
    c_phi = init_phi(ncvt, c_nodes);
    c_rho = cell(ncvt, 1);    
    
    for i = 1 : npnts
        pid = c_interPolySeed{i};
        mid = c_nnbEdgeIDX{i};         

        for j = 1 : length(mid)
            mmc = MMCs(mid(j), :);
            t  = mmc(1);
            sp = mmc(2:4);
            ep = mmc(5:7);

            for k = 1 : length(pid)
                ele = pid(k); 
                nodes = c_nodes{ele};

                phi = signed_distance(nodes, sp, ep, t);
                c_phi{ele} = max(phi, c_phi{ele});
            end
        end
    end

    faces = edofMat_mi(:, 3:3:end) / 3;

    % Heaviside
    parfor ele = 1 : ncvt
        H = Heaviside_simply(c_phi{ele}, epsilon); 
        c_rho{ele} = sum(H(faces), 2) / 8;
    end
end

function phi_cell = init_phi(ncvt, c_nodes)
    phi_cell = cell(ncvt, 1);
    parfor ele = 1:ncvt
        phi_cell{ele} = repmat(-1e5, size(c_nodes{ele}, 1), 1);
    end
end

function c_polyMesh = prep_polyMesh(edofMat, ref_nodes)

    nele  = size(edofMat, 1);
    nodes = reshape(ref_nodes', [], 1);
    c_polyMesh = cell(nele, 1);
    
%     figure; 

    parfor ele = 1 : nele
        edof = edofMat(ele,:);
        ne = nodes(edof);
        cbns = reshape(ne, 3, [])';

        faces = int32(convhull(cbns(:,1), cbns(:,2), cbns(:,3),'Simplify',true));
        vert1 = cbns(faces(:,1),:);
        vert2 = cbns(faces(:,2),:);
        vert3 = cbns(faces(:,3),:);
        c_polyMesh{ele} = [vert1, vert2, vert3]; % for the following intersection
        
%         [k1,av1] = convhull(cbns(:,1), cbns(:,2), cbns(:,3));
%         trisurf(k1,cbns(:,1), cbns(:,2), cbns(:,3));
%         hold on; scatter3(cbns(:,1), cbns(:,2), cbns(:,3));
%         hold on; 
    end
end


function display_3D(rho)
[nely,nelx,nelz] = size(rho);
hx = 1; hy = 1; hz = 1;            % User-defined unit element size
face = [1 2 3 4; 2 6 7 3; 4 3 7 8; 1 5 8 4; 1 2 6 5; 5 6 7 8];
set(gcf,'Name','ISO display','NumberTitle','off');
for k = 1:nelz
    z = (k-1)*hz;
    for i = 1:nelx
        x = (i-1)*hx;
        for j = 1:nely
            y = nely*hy - (j-1)*hy;
            if (rho(j,i,k) > 0.3)  % User-defined display density threshold
                vert = [x y z; x y-hx z; x+hx y-hx z; x+hx y z; x y z+hx;x y-hx z+hx; x+hx y-hx z+hx;x+hx y z+hx];
                vert(:,[2 3]) = vert(:,[3 2]); vert(:,2,:) = -vert(:,2,:);
                patch('Faces',face,'Vertices',vert,'FaceColor',[0.2+0.8*(1-rho(j,i,k)),0.2+0.8*(1-rho(j,i,k)),0.2+0.8*(1-rho(j,i,k))]);
                hold on;
            end
        end
    end
end
axis equal; axis tight; axis off; box on; view([30,30]); pause(1e-6);
end