%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-12-26 23:37:22
 % @LastEditTime: 2022-09-03 13:14:15

 % Variables Naming Notations:
 %   c_xx: cell   
%--------------------------
dbstop if error
feature accel on
% clear all
addpath util util_simulation util_mtimesx util_bezier util_multi_control_BC 
addpath util_micro util_trimesh util_output util_cvt util_MMC util_align util_splitEdges

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

volfrac = 0.4;
% optVcons = "pnorm";
optVcons = "overall";

t = 3; t0 = 0.1; t1 = 6; % for MMC
pb = 0.1; % for blending
epsilon = 0.1; % for Heaviside
step = [0.8*2, 0.8*2, 0.8*2]; % for Heaviside

e1 = 1; nu1 = 0.3; e0 = 1e-9; penal = 3;
vF = 1;

% optDesign = 'cantilever';
optDesign = 'Lshape'; 

nbridge = 0;

threshold = 1e-3; % threshold of triangle area, remove the non-valid triangle

scalarE = 0.3;
% scalarE = 1;

%% init
[nelx, nely, microx, bpoly, eps] = setting_info(optDesign);
globalx = nelx * microx;
globaly = nely * microx;
maxx = globalx / 2; minx = -maxx;
maxy = globaly / 2; miny = -maxy;

[cx, cy] = meshgrid(minx:1:maxx-1, miny:1:maxy-1);
cpnts = [cx(:), cy(:)] + 0.5;

datapath = "data/mat/cvt_mesh_"+optDesign+".mat";
if opt_load_cvt == 0
    [rho, cpnts] = init_trimesh_density(opt_load_rho, optDesign, vF, volfrac, ...
        nelx, nely, microx, bpoly); 

    if strcmp(optDesign, 'cantilever')
        seeds = generateCVT(nelx, nely, microx, rho); 
    else
        seeds = quadtree_nonregular(rho, cpnts, maxx, minx, maxy, miny, microx, eps, bpoly);
    end    

    % TODO: change to mexGen_cvt_mesh
    ncvt = size(seeds, 1);
    path0 = "data/cvt_"+optDesign+"_coarse/";
    [c_nodes, c_faces, c_vedges, c_poly] = generate_mesh_fromCVT(ncvt, path0, threshold);
    hold on; scatter(seeds(:,1), seeds(:,2), 8,'filled','k');
    
    save(datapath, "c_nodes", "c_faces", "c_vedges", "seeds", "c_poly", "rho", "cpnts");
else
    load(datapath);
end
% figure; voronoi(seeds(:,1), seeds(:,2)); axis equal;

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

[bdofs_cell, dofid_cell, nbnodes_cell] = border_check(c_faces, c_nodes, c_vedges);

% MMC
nd = 7; % [t1,t2,t3, x, y, len, sin(theta)]
T = repmat(t, npnts, 1);

[xval, xold1, xold2, xmin, xmax, low, upp, m_mma, c_mma, d_mma, ...
    a0_mma, a_mma] = MMA_paras(t, t0, t1, seeds, maxx, minx, maxy, miny);
    
% coarse mesh
phi_cell = parametric_B_cvt(nbnodes_cell, nbridge);

[edofMat, nvar, cbns] = assembleInfo_cvt(c_vedges, nbridge, nbnodes_cell);
U = zeros(nvar, 1);

[freedofs, fixeddofs, fext, ~] = design_domain_nonregular(cbns, optDesign, vF, ...
    minx, maxx, miny, maxy);

% fine mesh
[u1, l1, D1] = material_paras(e1, nu1);

[c_sK, c_area, area0] = prepare_sK(c_nodes, c_faces, 1, nu1);

rho1 = rho.^3;
% rho1 = rho;
% rho1(:) = 1;

%% iteration
datapath = "data/mat/cvt_mesh_"+optDesign+"_v"+volfrac+"_w"+scalarE+".mat";
if opt_load_mmc == 0
    
    loop = 0; change = 1; maxloop = 100;
    cLoop = zeros(maxloop,1);
    ELoop = zeros(maxloop,1);
    vLoop = zeros(maxloop,1);
    
    vars_loop = zeros(npnts, 3, maxloop);
    obj_loop = zeros(maxloop, 2);
    
%     figure(3); clf;
    
    while loop < maxloop && change > 1e-4
        loop = loop + 1; tic   
        
        vars_loop(:,:,loop) = [seeds, T];
    
        %% cvt
        conn_list = dt_connection(seeds);
    
        % compute projection of seeds - vedges
        indices = 1 : int32(npnts);
        [c_edges, vedges, c_nnbEdgeIDX, nnbSeedIDX] = intersection_info(seeds, ...
            indices', bpoly);
    
        [E, dE] = cvt_energy(seeds, cpnts, rho1, c_edges);
    
        %% MMC
        [MMCs, MMCs_poly] = update_MMCs(vedges, T, nnbSeedIDX, seeds);        

        if mod(loop,5)==0 || loop == 1
%             plot_MMC_cell(c_nnbEdgeIDX, MMCs_poly, 44);            
            T0 = repmat(1.5, npnts, 1);
            [~, MMCs_poly0] = update_MMCs(vedges, T0, nnbSeedIDX, seeds);
            plot_MMC(MMCs_poly0, maxx, minx, maxy, miny,optDesign,seeds,loop);
%             plotting_MMC(MMCs, maxx, minx, maxy, miny, pb,optDesign,loop);
        end
    
        [c_interPolyMMC, c_interPolySeed] = do_intersection(MMCs_poly, c_poly, c_nnbEdgeIDX);
    
        [c_rho, c_phi] = projection_independent(c_nodes, c_faces, MMCs, epsilon, ...
            c_nnbEdgeIDX, c_interPolySeed);
    
        % using current iteration c_interPoly for sensitivity
        [c_dH, c_interEles] = sensitivity_mmc_align(c_nodes, c_faces, seeds, step, T, ...
            bpoly, epsilon, conn_list, c_rho, c_phi, c_interPolySeed, maxx, minx, maxy, miny);
    
        %% simulation
        c_psi = preparation_cvt(e1, e0, penal, nbnodes_cell, phi_cell, dofid_cell, ...
            c_rho, c_faces, c_nodes, c_sK, edofMat, cbns);
    
        K = stiffness_cvt(c_rho, c_sK, c_psi, edofMat, nvar, e0, e1, penal);
        
        U(freedofs) = K(freedofs, freedofs) \ fext(freedofs);
        c = fext' * U;        
    
        [v_local, v] = volume_cvt(c_rho, c_area, optVcons, area0);
    
        %% optimization    
        [dc, dv] = sensitivity_cvt_align(c_dH, c_interEles, c_area, c_rho, c_psi, c_sK, ...
            edofMat, U, e0, e1, penal, area0);
    
        f0val= (1-scalarE) * c + E * scalarE; 
        df0dx= - dc * (1-scalarE) + dE * scalarE; 
        fval = v / volfrac - 1;
        dfdx = dv' / volfrac; 
        [xmma, ~, ~, ~, ~, ~, ~, ~, ~, low, upp] = mmasub_mmc(m_mma, nele, loop, xval, xmin, xmax, ...
            xold1, xold2, f0val, df0dx, fval, dfdx, low, upp, a0_mma, a_mma, c_mma, d_mma);
        xold2 = xold1; xold1 = xval; 
        
        OBJ(loop) = f0val;
        if loop >= 5 && (v-volfrac)/volfrac < 1e-3
            change = abs(max(abs(OBJ(loop-4:loop)-mean(OBJ(loop-4:loop))))/mean(OBJ(loop-4:loop)));
        end
%         change = max(abs(xval-xmma));  
    
        %% update seeds
        xval = xmma;
        vars = reshape(xval, 3, [])'; % [n,3]
        seeds = vars(:,1:2);
        T = vars(:,3);
        toc;
        
        cLoop(loop) = c;
        ELoop(loop) = E;
        vLoop(loop) = v;
        
        obj_loop(loop, :) = [c,E];
        
    %     figure(1); clf; voronoi(seeds(:,1), seeds(:,2)); axis equal;
%         plot_MMC(MMCs_poly, maxx, minx, maxy, miny);
        fprintf(' It.:%3i C.:%6.3f E.:%6.3f, V.:%6.4f ch.:%6.2f \n',loop,c,E,v,change);
    end
    plotting_MMC(MMCs, maxx, minx, maxy, miny, pb,optDesign,loop);
    save(datapath,"MMCs","cLoop","ELoop","vLoop","seeds","T","vars_loop","obj_loop");
else
    load(datapath);
    plotting_MMC(MMCs, maxx, minx, maxy, miny, pb,optDesign,loop);
end

%% utils
function plot_MMC_cell(c_nnbEdgeIDX, MMCs_poly, e)

    figure(3); hold on;

    nnbEdgeIDX = c_nnbEdgeIDX{e};
    for i = 1:size(nnbEdgeIDX, 1)        
        n = reshape(MMCs_poly(nnbEdgeIDX(i), :), 2, 4)';
        p = polyshape(n);
        plot(p,'FaceColor',[0.8,0.8,0.8],'FaceAlpha',1,'EdgeColor','none'); hold on;
%         plot(p,'FaceColor','r','FaceAlpha',1,'EdgeColor','none'); hold on;
    end
    axis equal; 
%     axes1 = gca;
%     set(axes1,'Xlim',[minx, maxx],'Ylim',[miny, maxy]);
    axis off;
    hold on;
end    

function [xval, xold1, xold2, xmin, xmax, low, upp, m_mma, c_mma, d_mma, ...
    a0_mma, a_mma] = MMA_paras(t, t0, t1, seeds, maxx, minx, maxy, miny)

    np = size(seeds, 1);

    T  = repmat(t, np, 1);

    xval = reshape([seeds, T]', [], 1); 
    xold1= xval; xold2 = xval; 

    xmin = repmat([minx, miny, t0], np, 1)'; 
    xmax = repmat([maxx, maxy, t1], np, 1)'; 
    xmin = xmin(:);
    xmax = xmax(:);

    low  = xmin; upp = xmax; 

    m_mma = 1; % number of constrain
    c_mma = 1000*ones(m_mma,1); d_mma = zeros(m_mma,1); a0_mma = 1; a_mma = zeros(m_mma,1); 
end