% addNum: add x points in one edge of macro-ele
function [multiU, multi_edofMat] = interpolateU(U, nelx, nely, addNum)

    [~, ~, edofMat] = forAssemble(nelx, nely);
    alldofs = 2*(nelx+1)*(nely+1) + 2 * (addNum-1)*nely*(nelx+1) + ...
                                    2 * (addNum-1)*nelx*(nely+1);
    %===================================================================

    multi_edofMat = zeros(nelx*nely, 4*2+addNum*4*2);
    multiU = zeros(alldofs, 1);

    for i = 1:nelx
        for j = 1:nely
            ele = j + (i-1)*nely;
            edof = edofMat(ele,:);
            ue = U(edof,:);

            % NOTE: squence for assemble
            um = zeros(4+addNum*4, 2);
            um(1:(addNum+1):end, :) = reshape(ue,2,4)'; % four orig nodes
            
            % four other interpolated points
            for edge = 1:4
                idx = (edge-1)*(addNum+1) + 1;
                for k = 1:addNum     
                    % linear interpoloation 
                    next = idx+addNum+1;
                    if idx+ addNum+1 > 4+addNum*4
                        next = 1;
                    end
                    um(idx+k, :) = um(idx, :) + (um(next, :) - um(idx, :))/(addNum+1)*k;
                end
            end

            multi_edof = interpolated_edof(i,j, addNum, nelx, nely, edof);

            multi_edofMat(ele, :) = multi_edof;
            multiU(multi_edof, 1) = reshape(um', 2*(4+addNum*4), 1);            
        end
    end

%     % test, addNum = 1;
%     figure;
%     for i = 1:nelx
%         for j = 1:nely
%             cx1 = i;     cy1 = j+1;
%             cx2 = i+0.5; cy2 = j+1;
%             cx3 = i+1;   cy3 = j+1;
%             cx4 = i+1;   cy4 = j+0.5;
%             cx5 = i+1;   cy5 = j;
%             cx6 = i+0.5; cy6 = j;
%             cx7 = i;     cy7 = j;
%             cx8 = i;     cy8 = j+0.5;
%             cx = [cx1, cx2, cx3, cx4, cx5, cx6, cx7, cx8];
%             cy = [cy1, cy2, cy3, cy4, cy5, cy6, cy7, cy8];
% 
%             ele = j + (i-1)*nely;
%             dof = multi_edofMat(ele, :);
%             u = reshape(multiU(dof, 1), 2, 8);
%             scatter(cx+u(1,:), cy+u(2,:),'filled'); hold on;
%         end    
%     end
end

% every macro ele: down -> right -> up -> left
% traverse all old-macro-dofs, then add new multi-dofs, for assemble Ktan
% the new dofs order is col by col either
function multi_edof = interpolated_edof(i,j, addNum, nelx, nely, edof)
    
    old_allNodes = (1+nelx)*(1+nely);

    n1 = (i-1)*addNum*nely + addNum*(nely+1)*(i-1) + (j-1)*addNum + old_allNodes; % left-up
    n2 =     i*addNum*nely + addNum*(nely+1)* i    + (j-1)*addNum + old_allNodes; % right-up

    multi_edof = [edof(1), edof(2)]; % left-down
    
    for k = 1:addNum
        % up, kth add points
        nk = i*addNum*nely + j + (addNum*(i-1) + k-1)*(nely+1) + old_allNodes;        
        multi_edof = [multi_edof, 2*(nk+1)-1, 2*(nk+1)]; % down
    end        

    multi_edof = [multi_edof, edof(3), edof(4)]; % right-down
    
    addpoints = 1:addNum;
    right = [2*(n2+flip(addpoints))-1; 2*(n2+flip(addpoints))];
    multi_edof = [multi_edof, right(:)']; % right
                        
    multi_edof = [multi_edof, edof(5), edof(6)]; % right-up

    for k = addNum:-1:1
        % up, kth add points
        nk = i*addNum*nely + j + (addNum*(i-1) + k-1)*(nely+1) + old_allNodes;
        multi_edof = [multi_edof, 2*nk-1, 2*nk]; % up
    end
    
    multi_edof = [multi_edof, edof(7), edof(8)]; % right-up    
    
    left = [2*(n1+addpoints)-1; 2*(n1+addpoints)];
    multi_edof = [multi_edof, left(:)']; % left
end