%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-01-07 12:07:54
 % @LastEditTime: 2021-01-08 16:31:49

 % precomputation for optimizing NH 
 % A & b contains constrain 1 & 2, since 1 & 2 are unrelated with displacement
%--------------------------
function [A, b, Q] = prepare_optNH(edofMat_ma, edofMat_mi, microx, dNh, vdofs)
        
    num_vdofs = size(edofMat_ma, 2);
    alldofs_mi = 2*(microx+1)^2;
    
    Q = obj_Q(dNh, edofMat_mi, microx, num_vdofs);

    [A, b] = cons_A(vdofs, alldofs_mi);

    % % compute step \omega
    % L = Z' * Q * Z;
    % rhs = - Z' * Q * x0;
    % omega = L \ rhs;

    % n = x0 + Z * omega;
    % n_opt = reshape(n, alldofs_mi, [])';
end

% rewrite constraints to a big Ax = b, i.e. [A1;A2;A3]x = [b1;b2;b3]
function [A, b] = cons_A(vdofs, num_ndofs)
    num_vdofs = length(vdofs);

    n1 = (1:num_vdofs*num_ndofs)';
    n2 = reshape(1:num_vdofs*num_ndofs, 8, []);

    % cons1: \sum_i n_ij = I
    I = eye(2);    
    R1 = repmat(I, 1, num_vdofs/2); % [2, 2v] 
    
    I = eye(num_ndofs);
    A1 = kron(I, R1); % [2,2v] -> [2*2n, 2v*2n]
    % A1 = kron(R1, I); % [2,2v] -> [2*2n, 2v*2n], NOTE: the sequence

    C1 = repmat(eye(2), 1, num_ndofs/2); % [2, 2n]
    b1 = C1(:);

    % ttt = R1 * n2 - C1;
    % ttt = A1 * n1 - b1;
    % % max(ttt(:) - ttt1(:))
    % save ttt ttt
    
    % cons2: n_ij = \delta_ij * I, for all X_i^H, X_j^H
    R2 = zeros(num_vdofs, num_ndofs); % [2v, 2n]
    R2(:, vdofs) = eye(num_vdofs);
    
    I2 = eye(num_vdofs);
    A2 = kron(R2, I2); % [2v*2v, 2v*2n]
    % A2 = kron(I2, R2); % [2v*2v, 2v*2n]

    C2 = eye(num_vdofs);
    b2 = C2(:);

    A = [A1; A2];
    b = [b1; b2];
    
    % ttt = A2 * n1 - b2;
    % save ttt ttt

    % % cons3: micro deformation
    % I3 = eye(num_ndofs);
    % A3 = kron(u_ma', I3); % [1,2v] -> [2n,2n*2v], NOTE: the sequence

    % b3 = u_mi;

%     % compute
%     A0 = [A1; A2; A3];
%     A = sparse(A0);
%     b = sparse([b1; b2; b3]);
%     x0 = A\b; % initial feasible solution
    
% %     tic
% %     Z = null(A0);
% %     toc
    
%     % maybe big matrix could be used
%     tic
%     [~, SpRight] = spspaces(A,3,10^-10);
%     toc
%     Q = SpRight{1};
%     J = SpRight{3};
%     Z = Q(:,J); % null-space matrix of A        
end

% expand orignal n:[2n,2v] to [2n*2v,1]
% min \sum tr(n^T * dNh^T * dNh * n) = \sum tr(n^T * Q * n)
function Q = obj_Q(dNh, edofMat_mi, microx, num_vdofs)
    eleNum_mi = microx^2;
    alldofs_mi = 2*(microx+1)^2;

    % for obj, dNh: [4,8] -> [4m,8m] -> [4m,2n] ->[4m*2v,2n*2v]
    Q = 0;
    I1 = eye(size(edofMat_mi, 1)); % [m,m]
    I2 = eye(num_vdofs);           % [2v,2v]

    % n_tmp = reshape(1:num_vdofs*alldofs_mi, alldofs_mi, []); 
    % n1 = 1:num_vdofs*alldofs_mi;

    for gp = 1:4
        dNhg = dNh{gp};
        dNh1 = kron(I1, dNhg); % [4m,8m]
        
        dNh2 = zeros(4*eleNum_mi, alldofs_mi); % [4m,2n]
        for ele = 1:eleNum_mi
            edof = edofMat_mi(ele, :);
            dNh2(:,edof) = dNh2(:,edof) + dNh1(:,8*(ele-1)+1:8*ele);
        end

        % post multiplcation need change kron sequence
        dNh_opt = sparse(kron(dNh2, I2)); % [4m*2v, 2n*2v]

        % dNh_opt = sparse(kron(I2, dNh2));

        % ttt{gp} = dNh_opt * n1';
        % ttt{gp} = reshape(n1, 8, []) * dNh2'; % [2v,4m]
%         ttt1 = reshape(dNh_opt * n1', [], num_vdofs);
%         max(ttt1(:) - ttt{gp}(:))
        Q = Q + dNh_opt' * dNh_opt;
    end
    % save ttt ttt
end

% NOTE: this func is for optNH, not for fast_optNH using QR frac
% prepare R12 considering fixeddofs
% for those micro dofs influenced by fixeddofs_ma, they don't have to be summed as 1
function R12 = cons1_R12(fixeddofs, edofMat_ma, corners, alldofs_mi)
    eleNum_ma = size(edofMat_ma, 1);
    fixed  = fixed_ele(fixeddofs, edofMat_ma);
    R12 = cell(eleNum_ma, 1);
    parfor ele = 1:eleNum_ma
        if fixed(ele)
            % find fixeddofs in this macro-ele 
            edof = edofMat_ma(ele, :);
            shared_dofs = intersect(fixeddofs, edof);

            % find corresponding micro-dofs
            shared_num = length(shared_dofs);
            micro_dofs = zeros(shared_num, 1);
            for i = 1:shared_num
                micro_dofs(i) = corners(edof == shared_dofs(i));
            end

            freedofs_mi = setdiff(1:alldofs_mi, micro_dofs);
            
            I = eye(alldofs_mi - shared_num); 
            tmp = zeros(alldofs_mi, alldofs_mi - shared_num);
            tmp(freedofs_mi, :) = I;
            R12{ele} = tmp;
        else
            R12{ele} = eye(alldofs_mi);
        end
    end
end