%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-10-06 14:42:41
 % @LastEditTime: 2021-12-07 12:08:53

 % only optimize one t1, assume t1=t2=t3
%--------------------------
function [MMCs, phi_max] = optimize_thickness(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;
    cnum = size(MMCs, 2);
    
    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;
    [xval, xold1, xold2, xmin, xmax, low, upp, m_mma, c_mma, d_mma, a0_mma, a_mma] = MMA_paras1(MMCs, MMCs_min, MMCs_max);   
    Loop=1; change=1; cons_v_micro = 1; maxiter = 100;

    while Loop < maxiter && (change > 1e-4 || cons_v_micro > volfrac)
        
        [H, diffH, phi_max] = projection_mmc(coorx, coory, MMCs, dx, cnum, 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;    
        figure(3); contourf(coorx, coory, phi_max,[0,0]); axis equal; axis off; pause(1e-6);

        [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) ;
        
        % 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(cnum, 1); 
        dfdx = zeros(cnum, 1); 
        for k=1:cnum 
            tmp1 = 2*energy_nod'.*H * squeeze(diffH(k, :))'; 
            df0dx(k)=tmp1;             

            dHv = diffH(k, :); % [nnode_global, 1]
            dH1 = sum(dHv(EleNodesID), 2) / 4; % [nele_global, 1]
            dfdx(k) = sum(dH1 .* dv_micro);
            
%             dfdx(:, k) = squeeze(sum(diffH(k, :, :), 3) / 4);
        end
        
        % MMA optimization 
        f0val= comp; 
        df0dx= -df0dx(:) / max(abs(df0dx(:))); 
        fval = cons_v_micro - volfrac;
        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, :) = round(xval'*1e4)/1e4; 

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

end

function [H, diffH, phi_max] = 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);
    phi0 = 0;
    phi2 = 0;
    for ci = 1 : 2
        mmc = MMCs(:, ci);
        phi = tPhi(mmc, coorx, coory);
%         phi(phi>0) = 1;
%         phi(phi<0) = -1;
        phi_max = max(phi_max, phi);
        phi_cell{ci} = phi;
        
%         phi0 = phi0 + phi;
%         phi2 = phi2 + phi.^2;
        
%         phi0 = phi0 + phi.^(15);
%         figure(1); colormap(gray); imagesc(1-flipud(phi)); caxis([0 1]); axis equal; axis off; drawnow;
%         figure(3); contourf(coorx, coory, phi,[0,0]); axis equal; axis off; pause(1e-6);
    end
    
    p = 2;
    phi0 = 0;
    for ci = 1 : 2
        phi = phi_cell{ci};
        phi0 = phi0 + exp(p * (phi - phi_max));
    end
    phi = 1/p * log(phi0) + phi_max;

%     phi = phi0 + phi2.^(1/2);
%             phi(phi>0) = 1;
%         phi(phi<0) = -1;
%     phi = phi0.^(1/15);
    figure(1); colormap(gray); imagesc(1-flipud(phi)); axis equal; axis off; drawnow;
    figure(3); contourf(coorx, coory, phi,[0,0]); axis equal; axis off; pause(1e-6);
    
%     phi_max(:) = (H_filter*phi_max(:)) ./ Hs_filter;    

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

    % sensitivity
    step  = [repmat(0.01, 1, 3), repmat(max(2*dx, 0.005), 1, 4)];
    diffH = zeros(cnum, (globaly+1)*(globalx+1)); 
    for ci = 1 : cnum
        mmc = MMCs(:, ci);       

        mmc_plus = mmc; 
        mmc_plus(1) = mmc(1) + step(1);
        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(1) = mmc(1) - step(1); 
        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, :) = (HD1-HD2) / (2*step(1));         
    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)
    % variables are t1,t2,t3
    xval = MMCs(1, :); 
    xval = xval(:);
    xold1= xval; xold2 = xval; 
    xmin = MMCs_min(1, :); 
    xmax = MMCs_max(1, :); 
    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