%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2022-01-14 21:23:30
 % @LastEditTime: 2022-05-03 19:23:41
%--------------------------
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

%% adjustable paras
% opt_load_cvt = 0;
% opt_load_mmc = 0;

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

nbridge = 0;

scalarE0 = 0.2;

optDesign = 'cantilever';
optTest = 'box';
% optDesign = 'cantilever_hole';

nelx = 4;
nely = 2; 
nelz = 1;
microx = 64;
dx = 1;

%% init
lx = dx * microx;
globalx = nelx * microx;
globaly = nely * microx;
globalz = nelz * microx;
maxx = globalx / 2; minx = -maxx;
maxy = globaly / 2; miny = -maxy;
maxz = globalz / 2; minz = -maxz;

% datapath = "data/mat/cvt_mesh_"+optDesign+".mat";
% if opt_load_cvt == 0
    
    spath = "F:\\project\\7-current_opt\\opt-cvt-3D\\data\\seeds_box.cin";
    seeds = load_cin(spath);
    
    [c_polyMesh, c_nodesFine, c_tets, c_seg, c_polygon] = ...
        generateMesh(seeds, nelx, nely, nelz, microx, optTest);

%     save(datapath, "c_nodesFine", "c_tets", "c_seg", "seeds", "c_polygon", ...
%         "c_polyMesh");
% else
%     load(datapath);
% end
% figure; scatter3(seeds(:,1), seeds(:,2), seeds(:,3), 'filled'); axis equal;
% load seeds % cvt init version

nele = numel(seeds) + size(seeds, 1);


% epsilon = compute_epsilon(c_tets, c_nodesFine);
% step = epsilon * 2;
epsilon = 2;

[c_bnid, c_dofid, c_bnidSaved, c_bnidSavedID, nbnidFine] = border_check(c_nodesFine, c_polygon);

%% coarse mesh
[c_edofMat, c_nodeCoarse, cbns, c_nid, c_dupB] = assemble_cvt(c_polygon);
% figure; scatter3(cbns(:,1), cbns(:,2), cbns(:,3)); hold on;  

c_phi = parametric_B_cvt(c_polygon, c_nodesFine, c_bnid, c_bnidSaved, ...
    c_bnidSavedID, c_edofMat, c_nid, c_dupB, nbnidFine, cbns, c_nodeCoarse);

[freedofs, fixeddofs, fext, alldofs] = design_domain(optDesign, maxx, minx, ...
    maxy, miny, maxz, minz, vF, cbns, 0);
U = zeros(alldofs, 1);

%% fine mesh
[c_sK, c_vol, vol0] = prepare_sK(c_nodesFine, c_tets, 1, nu0);

%% iteration
datapath = "data/mat/error_"+optDesign+"_v"+volfrac+".mat";
load(datapath);

loop = 0; maxloop = 100;

while loop < maxloop
    loop = loop + 1; tic   

    seeds = vars_loop(:,1:3,loop);
    T     = vars_loop(:,4,loop);
    npnts = size(seeds, 1);
    
    %% cvt
    indices = 1 : int32(npnts);
    [c_edges, cenP, vedges, c_nnbEdgeIDX, c_nnbSeedIDX] = intersection_info(seeds, ...
        indices, nelx, nely, nelz, microx,optDesign);   

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

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

    [c_rho, c_dist] = projection_independent(c_nodesFine, c_tets, MMCs, ...
        epsilon, c_nnbEdgeIDX, c_interPolySeed);        

    %% simulation
    c_psi = preparation_cvt(e1, e0, penal, c_phi, c_dofid, c_rho, c_tets, c_sK);

    K = stiffness_cvt(c_rho, c_sK, c_psi, c_edofMat, alldofs, e0, e1, penal, c_vol);
    
    U(freedofs) = K(freedofs, freedofs) \ fext(freedofs);
    c_CBN_vor = fext' * U;

    %% comparison
    c_global = comp_loop(loop, 4);
    cbn_vor_err = (c_CBN_vor-c_global)^2 / c_global^2;

    comp_loop(loop, 2)  = c_CBN_vor;
    error_loop(loop, 2) = cbn_vor_err;
    fprintf(' It.:%3i Er.:%6.2f \n',loop,cbn_vor_err);
end 
save(datapath, 'comp_loop', 'error_loop');

% output_mmc(MMCs, maxx, minx, maxy, miny, maxz, minz, loop);
% else
%     load(datapath);
% end

%% utils
function seeds = load_cin(datapath)
    
    fileID = fopen(datapath, 'r');
    info = fscanf(fileID, '%f');
    seeds = reshape(info, 3, [])';

    fclose(fileID);
end

function plot_mmc(MMCs, minx, maxx, miny, maxy, minz, maxz)
    figure(1); clf;
%     for i = 1:size(MMCs, 1)
%         t = MMCs(i, 1);
        x = MMCs(:,[2,5]);
        y = MMCs(:,[3,6]);
        z = MMCs(:,[4,7]);
        plot3(x',y',z','LineWidth',2, 'Color','k'); hold on;  
%     end
    view([10 25]);
    xlabel('x axis');
    ylabel('y axis');
    zlabel('z axis');
    axis equal;
    axes1 = gca;
    set(axes1,'Xlim',[minx, maxx],'Ylim',[miny, maxy], 'Zlim',[minz, maxz]);
%     axis off;
end

function defn1 = extract_def_nodes(U, c_polygon, cbns)

    defn = reshape(U, 3, [])' + cbns;
    defn1 = [];
    for i = 1:size(c_polygon)
        segs = c_polygon{i};
        for j = 1:size(segs, 1)
            nodes = segs{j}(:, 1:3);
    
            [~, idx] = ismembertol(nodes, cbns, 1e-3, 'Byrows', true); 
            idx1 = idx(idx > 0);
            defn1 = [defn1; defn(idx1, :)];
        end
    end
    figure; scatter3(defn1(:,1), defn1(:,2), defn1(:,3));
end

function epsilon = compute_epsilon(c_tets, c_nodesFine)

    len2 = zeros(size(c_tets,1), 1);
    for ele = 1:size(c_tets,1)
        tets = c_tets{ele};
        nodes = c_nodesFine{ele};
        x = nodes(:,1);
        y = nodes(:,2);
        z = nodes(:,3);
        x = x(tets);
        y = y(tets);
        z = z(tets);
    
        len1 = sqrt((x(:,1) - x(:,2)) .^ 2 + (y(:,1) - y(:,2)) .^ 2 + (z(:,1) - z(:,2)) .^ 2);
        len2(ele) = mean(len1);
    end
    epsilon = mean(len2) / 2;
end