%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-08-25 17:26:51
 % @LastEditTime: 2021-09-02 19:42:17
%--------------------------
function [f0val, df0dx, fval, dfdx] = obj_cons_sens(H_macro, Hs_macro, H_global, Hs_global, fext, U, nele_global, v0_macro, v0_micro, dK, cons_num, ...
    dk_micro, edofMat_ma, rho_cell, rho_macro, rho_global, e0,e1,penal)

    pnorm = 16;
    nele_macro = length(rho_macro(:));
    nele_micro = nele_global / nele_macro;
    vdofs_num = size(edofMat_ma, 2);

    %% compliance & volume_macro
    comp = fext' * U;
    cons_v_macro = mean(rho_macro(:)) / v0_macro - 1;

    % sensitivities - macro
    dc_macro = zeros(nele_macro, 1);
    dv_macro = ones(nele_macro, 1) / nele_macro / v0_macro;

    % micro
    volume_micro = zeros(nele_macro,1);

    for ele_ma = 1:nele_macro
        rho_micro = rho_cell{ele_ma};
        volume_micro(ele_ma) = mean(rho_micro(:));

        edof = edofMat_ma(ele_ma, :);
        ue = U(edof);

        dKe = reshape(dK(:, ele_ma), vdofs_num, vdofs_num);
        dc_macro(ele_ma) = ue' * dKe * ue;
    end
%     cons_v_micro = (mean(volume_micro.^pnorm))^(1/pnorm) / v0_micro - 1;
%     
%     dpnorm = 1/nele_macro * (mean(volume_micro.^pnorm))^(1/pnorm-1) * (volume_micro.^(pnorm-1)) / v0_micro;    
%     dpn_expand = kron(ones(nele_micro, 1), dpnorm); % expand to [nele_global, 1]
%     dv_micro  = ones(nele_global, 1) / nele_global .* dpn_expand;
    
    cons_v_micro = mean(rho_global(:)) / v0_micro - 1;
    dv_micro  = ones(nele_global, 1) / nele_global / v0_micro;

    [dc_micro] = obj_sens_micro(rho_macro, dk_micro, U, edofMat_ma, e0,e1,penal);
    
    dc_micro(:) = H_global * (dc_micro(:) ./ Hs_global);
    dv_micro(:) = H_global * (dv_micro(:) ./ Hs_global);
    % CHECK: maybe need to be scaled to 1000 times    

    dc_macro(:) = H_macro * (dc_macro(:) ./ Hs_macro);
    dv_macro(:) = H_macro * (dv_macro(:) ./ Hs_macro);

    %% sensitivities
    df0dx = zeros(nele_macro + nele_global, 1);
    dfdx  = zeros(cons_num, nele_macro + nele_global);
    
    f0val = comp;
    df0dx(1 : nele_macro)     = - dc_macro(:); % note the negetive sign
    df0dx(nele_macro+1 : end) = - dc_micro(:); % note the negetive sign
    
    fval(1) = cons_v_macro;
    dfdx(1, 1 : nele_macro) = dv_macro;
    fval(2) = cons_v_micro;
    dfdx(2, nele_macro+1 : end) = dv_micro(:);
%     fval = cons_v_micro;
%     dfdx(1, :) = dv_micro(:);
end

% compliance_micro sensitivities
function [dc_micro] = obj_sens_micro(macro_rho, dk_micro, U, edofMat_ma, e0,e1,penal)

    nele_macro = size(macro_rho(:), 1);
    nele_micro = size(dk_micro{1}, 2);
    vdofs_num = size(edofMat_ma, 2);
%     jac = dx^2/4;

    dc_micro = zeros(nele_micro * nele_macro, 1);

    for ele_ma = 1:nele_macro
%         rho_e = rho_cell{ele_ma}(:);
%         tmp = speye(length(rho_e)) .* (e0 + rho_e.^penal * (e1-e0)); % m,m micro-rho need to be penal
%         D = kron(tmp, D1); % 3m,3m

        dk = dk_micro{ele_ma}; % [6r^2, nele_micro]
        
        edof = edofMat_ma(ele_ma, :);
        ue = U(edof);
        for ele_mi = 1:nele_micro
            global_ele = (ele_ma-1) * nele_micro + ele_mi;
            dk_mi = reshape(dk(:, ele_mi), vdofs_num, []);
            dc_micro(global_ele) = ue' * dk_mi * ue * (e0 + macro_rho(ele_ma)^penal * (e1-e0));
        end
    end
end