%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-10-06 14:42:41
 % @LastEditTime: 2021-10-07 10:33:23

 % optimize variables depend on vnum
 % could be 7 variables or t1&t2&t3 or t1
%--------------------------
function MMCs = optimize_mmc(cnum, vnum, maxiter, coorx, coory, dx, MMCs, MMCs_min, MMCs_max, edofMat, iK, jK, KE, ...
    freedofs, F, global2cell_id, nele_micro, nele_macro, volfrac, H_filter, Hs_filter)

    pn = 16;
    globaly = size(coorx, 1) - 1;
    globalx = size(coorx, 2) - 1;
    nele_global = nele_micro * nele_macro;
    U = zeros(2*(globaly+1)*(globalx+1), 1); 
    
    EleNodesID = edofMat(:,2:2:8)./2; 
    iEner = EleNodesID'; 

    nn = cnum * vnum;
    [xval, xold1, xold2, xmin, xmax, low, upp, m_mma, c_mma, d_mma, a0_mma, a_mma] = MMA_paras1(MMCs, MMCs_min, MMCs_max, vnum);   
    Loop=1; change=1; 

    while Loop < maxiter && change > 1e-3
        
        [H, diffH] = projection_mmc(coorx, coory, MMCs, dx, cnum, vnum, H_filter, Hs_filter);

        H1 = reshape(H, globaly+1, globalx+1); 
        figure(2); colormap(gray); imagesc(1-flipud(H1)); caxis([0 1]); axis equal; axis off; drawnow;    

        [den, energy_nod, comp] = simulation_MMC(edofMat, EleNodesID, iEner, H, iK, jK, KE, freedofs, U, F);

%         figure(1); colormap(gray); imagesc(1-flipud(reshape(den, globaly, globalx))); caxis([0 1]); axis equal; axis off; drawnow;
        
%         cons_v_micro = sum(den)/nele_global - volfrac;

        % p-norm volume constraint
        rho_cell = reshape(den(global2cell_id), nele_micro, nele_macro);
        volume_micro = mean(rho_cell, 1)'; % [m,M] -> [1,M]
        cons_v_micro = (mean(volume_micro.^pn))^(1/pn) - volfrac;
        
        % Sensitivities 
        dpnorm = 1/nele_macro * (mean(volume_micro.^pn))^(1/pn-1) * (volume_micro.^(pn-1));
        dpn_expand = kron(ones(nele_micro, 1), dpnorm); % expand to [nele_global, 1]
        dv_micro  = ones(nele_global, 1) / nele_global .* dpn_expand;

        df0dx= zeros(vnum, cnum); 
        dfdx = zeros(vnum, cnum); 
        for k=1:cnum 
            tmp1 = 2*energy_nod'.*H * squeeze(diffH(k,:,:))'; 
            df0dx(:, k)=tmp1; 
            
            for vi = 1:vnum
                dHv = diffH(k, vi, :); % [nnode_global, 1]
                dH1 = sum(dHv(EleNodesID), 2) / 4; % [nele_global, 1]
                dfdx(vi, k) = sum(dH1 .* dv_micro);
            end
%             dfdx(:, k) = squeeze(sum(diffH(k, :, :), 3) / 4);
        end
        
        %MMA optimization 
        f0val= comp; 
        df0dx= -df0dx(:) / max(abs(df0dx(:))); 
        fval = cons_v_micro;
        dfdx = dfdx(:) / max(abs(dfdx(:))); 
        [xmma, ~, ~, ~, ~, ~, ~, ~, ~,low, upp]= mmasub_mmc(m_mma, nn, Loop, xval(:), xmin, xmax, xold1,...
            xold2, f0val, df0dx, fval, dfdx', low, upp, a0_mma, a_mma, c_mma, d_mma);
        xold2 = xold1; xold1 = xval; 
        change  = max(abs(xval-xmma)); 

        % update
        xval = xmma;
        MMCs(1:vnum, :) = reshape(round(xval*1e4)/1e4, vnum, []); 

        fprintf(' It.:%4i Obj.:%6.3f Vol.:%6.4f ch.:%6.4f \n',Loop,f0val,fval+volfrac,change);
        Loop=Loop+1; 
    end

end

function [H, diffH] = projection_mmc(coorx, coory, MMCs, dx, cnum, H_filter, Hs_filter)
    globaly = size(coorx, 1) - 1;
    globalx = size(coorx, 2) - 1;
    
    phi_max = repmat(-1e5, size(coorx));
    phi_cell = cell(cnum, 1);
    
    for ci = 1 : cnum
        mmc = MMCs(:, ci);
        phi = tPhi(mmc, coorx, coory);
        phi_max = max(phi_max, phi);
        phi_cell{ci} = phi;
    end

%     phi_max(:) = (H_filter*phi_max(:)) ./ Hs_filter;
    figure(3); contourf(coorx, coory, phi_max,[0,0]); axis equal; axis off; pause(1e-6);

    epsilon = 4 * dx;
    H = Heaviside(phi_max, globalx, globaly, epsilon); 
    H(:) = (H_filter*H(:)) ./ Hs_filter;

    % sensitivity
    step  = max(2*dx, 0.005);
    diffH = zeros(cnum, vnum, (globaly+1)*(globalx+1)); 
    for ci = 1 : cnum
        mmc = MMCs(:, ci);

        for vi = 1:vnum

            mmc_plus = mmc; 
            mmc_plus(vi) = mmc(vi) + step;
            tmpPhiD1 = tPhi(mmc_plus, coorx, coory); 
            phi_plus = tmpPhiD1; 
            for ik=1:ci-1 
                phi_plus = max(phi_plus, phi_cell{ik}); 
            end
            for ik=ci+1:cnum 
                phi_plus=max(phi_plus, phi_cell{ik}); 
            end

            mmc_minus = mmc; 
            mmc_minus(vi) = mmc(vi) - step; 
            tmpPhiD2 = tPhi(mmc_minus, coorx, coory); 
            phi_minus=tmpPhiD2; 
            for ik=1:ci-1 
                phi_minus=max(phi_minus, phi_cell{ik}); 
            end
            for ik=ci+1:cnum
                phi_minus=max(phi_minus, phi_cell{ik}); 
            end
    %             phi_plus(:) = (H_filter*phi_plus(:)) ./ Hs_filter;
    %             phi_minus(:) = (H_filter*phi_minus(:)) ./ Hs_filter;
            HD1 = Heaviside(phi_plus, globalx,globaly,epsilon); 
            HD2 = Heaviside(phi_minus, globalx,globaly,epsilon); 
            HD1(:) = (H_filter*HD1(:)) ./ Hs_filter;
            HD2(:) = (H_filter*HD2(:)) ./ Hs_filter;
            diffH(ci, vi, :) = (HD1-HD2) / (2*step);         
        end
    end
end

function [xval, xold1, xold2, xmin, xmax, low, upp, m_mma, c_mma, d_mma, a0_mma,  a_mma] = MMA_paras1(MMCs, MMCs_min, MMCs_max, vnum)
    % variables are t1,t2,t3
    xval = MMCs(1:vnum, :); 
    xval = xval(:);
    xold1= xval; xold2 = xval; 
    xmin = MMCs_min(1:vnum, :); 
    xmax = MMCs_max(1:vnum, :); 
    xmin = xmin(:); xmax = xmax(:);
    low  = xmin; upp = xmax; 

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