%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2022-05-10 15:53:23
 % @LastEditTime: 2022-05-12 11:24:11
%--------------------------
function recover_def_mmc(c_phi, U, c_regulated, edofMat_ma, c_nodesFine, ...
    microx, nelx, nely, nelz, dofid, MMCs)

    microx = 50;
    
    for i = 1:nelx
        for j = 1:nely
            for k = 1:nelz
                ele  = (k-1)*nelx*nely + (i-1)*nely + j;
                edof = edofMat_ma(ele, :);

                ue = U(edof);
                he = c_regulated{ele} * ue; % he order: [4-borders, inner]
                umicro = he(dofid);

                nodes = c_nodesFine{ele};
                def_fine = nodes + reshape(umicro, 3, [])';

                % 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);
                [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);
        
                % interpolation for the deformed shape
                Fx = scatteredInterpolant(nodes(:,1),nodes(:,2),nodes(:,3),def_fine(:,1),'natural');
                Fy = scatteredInterpolant(nodes(:,1),nodes(:,2),nodes(:,3),def_fine(:,2),'natural');
                Fz = scatteredInterpolant(nodes(:,1),nodes(:,2),nodes(:,3),def_fine(:,3),'natural');

                vqx = Fx(verts); 
                vqy = Fy(verts);
                vqz = Fz(verts);
                stlwrite(['data/stl/output1-test',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-test',num2str(ele),'.stl'], faces, [vqx,vqy,vqz]);
            end
        end
    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