%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2020-12-03 22:31:02
 % @LastEditTime: 2021-01-08 16:02:26

 % optimize n to get a ideal macro shape function
 % vdofs_ma: idx in micro-ndofs which matches to macro-vars
%--------------------------
function n_opt = optNH(u_mi, u_ma, vdofs_ma, microx, harm_u, edofMat, dNh, opt_harmonic)
    %% yalmip init
    ops = sdpsettings('solver', 'mosek');
    ops.showprogress = 0; 
    ops.verbose = 0; 
    ops.debug = 1;
    ops.warning = 1;
    % ops.removeequalities = 1; % removes equality constraints using a QR decomposition and reformulates the problem using a smaller set of variables
    
    num_v       = length(vdofs_ma)/2;
    num_dofs_mi = 2*(microx+1)^2;
    harm_num    = size(harm_u, 2);

    n = sdpvar(num_v*2, num_dofs_mi); % [2v, 2n]
%     n = reshape(1:num_v*2*num_dofs_mi, [], num_dofs_mi);
    
    %% obj 
    tmp_mat = edofMat';
    n_reorder = n(:, tmp_mat(:))'; % [2v,8m]'
    n1 = reshape(n_reorder, 8, []); % [8,m*2v]
    obj = 0;
    for gp = 1:4
        dNh_g = dNh{gp};  % [4,8]
        dNH = dNh_g * n1; % [4,m*2v]
        dNH2 = reshape(dNH, [], 2*num_v); % [4*m,2v]
        % dNH1 = reshape(dNH, 4, [], 2*num_v); % [4,m,2v]
        % dNH2 = squeeze(sum(dNH1, 2)); % [4,2v]
        obj = obj + trace(dNH2 * dNH2');
    end

    %% cons
    % cons1: \sum_i n_ij = I
    I = eye(2);    
    R1 = repmat(I, 1, num_v);         % [2, 2v] 
    
    C1 = repmat(eye(2),1,num_dofs_mi/2);
    cons1 = [R1 * n == C1];
    
%     freedofs_num = size(R12,2);
%     C1 = repmat(I, 1, freedofs_num/2); % [2, 2n]    
%     cons1 = [R1 * n * R12 == C1];

    % cons2: n_ij = \delta_ij * I, for all X_i^H, X_j^H
    R2 = zeros(num_dofs_mi, 2*num_v); % [2n, 2v]
    R2(vdofs_ma, :) = eye(2*num_v);
    C2 = eye(num_v*2);
    cons2 = [n * R2 == C2];

%     tmp = (n*R2 - C2)';
%     load ttt
%     max(tmp(:) - ttt(:))

    if opt_harmonic
        % cons3: harmonic deformation
        cons3 = [];
        for i = 1:harm_num
            C3 = harm_u(:, i);        % [2n, 1]
            R3 = harm_u(vdofs_ma, i); % [2v, 1]
            cons3 = [cons3, R3' * n == C3']; % [1,2v] * [2v,2n] = [1,2n]
        end

        cons = [cons1, cons2, cons3];
    else    
        % cons3: micro deformation
        cons4 = u_ma' * n == u_mi';
        cons = [cons1, cons2, cons4];
    end
        
    %% opt
    sol = optimize(cons, obj, ops);   
                 
%     % Analyze error flags
    if sol.problem == 0
        n_opt = value(n); 
%         % Extract and display value
%         u_opt = value(u_var);
%         obj_opt = value(obj)
%         
%         err = sum((u_opt-u_obj(:)).^2) / sum(u_obj(:).^2)
    else
         sol.info
         yalmiperror(sol.problem)
    end   
end