%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2021-02-22 16:13:23
 % @LastEditTime: 2022-05-10 19:43:07
 
 % Global node-ID is ordered as y(1-0) -> x(0-1) -> z(0-1)
 %   y
 %   |         
 %   0---x
 %  /
 % z
%--------------------------
function [freedofs, fixeddofs, fext, alldofs] = design_domain(opt_design, ...
    maxx, minx, maxy, miny, maxz, minz, vF, nodes, optChanged)

    nodes(:,4) = 1:size(nodes,1);

    switch opt_design       
    case 'halfsp'
        vF = -vF;

        r = (maxx - minx) / 5;
        midx = (maxx - minx) / 2 + minx;
        midz = (maxz - minz) / 2 + minz;
        [loadnid, ~] = find(abs(nodes1(:,2)-maxy)<r & abs(nodes1(:,1)-midx)<r & abs(nodes1(:,3)-midz)<r);
        loaddofs = 3*loadnid(:) - 1;
        
        [fixednid, ~] = find(abs(nodes1(:,2)-miny)<1e-5);
        fixeddofs = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';

    case 'cylinder'
        vF = -vF;        
        
        % two-scale            
        [loadnid, ~] = find(abs(nodes(:,3)-maxz)<1e-5 & abs(nodes1(:,2)-miny)<1e-5 & abs(nodes1(:,1))<1e-5);
        loaddofs = 3*loadnid(:) - 1;

        [ix,~] = find(abs(nodes(:,3)-minz)<1e-5);
        fixednid = nodes(ix, 4);
        fixeddofs = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';       

    case 'MBB'
        vF = -vF;

        nodes(:,4) = 1:size(nodes,1);
        midz = (maxz + minz) / 2;
        v = min(abs(nodes(:,3)-midz));
        [iz,~] = find(abs(nodes(:,3)-v-midz)<1e-3); 
        nodes1 = nodes(iz, :);
        [iy,~] = find(nodes1(:,2)==maxy);
        nodes2 = nodes1(iy, :);
        [ix,~] = find(nodes2(:,1)==0);        
        loadnid = nodes2(ix, 4);
        loaddofs = 3*loadnid(:) - 1;

        % [maxx, 0, [0,maxz]]        
        [ix,~] = find(nodes(:,1)==maxx);
        nodes1 = nodes(ix, :);
        [iy,~] = find(nodes1(:,2)==0);
        nodes2 = nodes1(iy, :);
        [iz1,~] = find(nodes2(:,3)==0);
        [iz2,~] = find(nodes2(:,3)==maxz);                
        fixednid = nodes2([iz1, iz2], 4);
        fixeddofs1 = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';       
        % [0,:,:]
        [ix,~] = find(nodes(:,1)==0);
        fixednid = nodes(ix, 4);
        fixeddofs2 = [3*fixednid(:), 3*fixednid(:)-2]';
        
        fixeddofs = [fixeddofs1(:); fixeddofs2(:)];
        
    case 'cantilever'
        vF = -vF;        
        % two-scale            
        nodes(:,4) = 1:size(nodes,1);
        [ix,~] = find(abs(nodes(:,1)-maxx)<1e-5);
        nodes1 = nodes(ix, :);
        [iy,~] = find(abs(nodes1(:,2)-miny)<1e-5);
        nodes2 = nodes1(iy, :);
        loadnid = nodes2(:, 4);
        loaddofs = 3*loadnid(:) - 1;

        [ix,~] = find(abs(nodes(:,1)-minx)<1e-5);
        fixednid = nodes(ix, 4);
        fixeddofs = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';       
        
    case 'cantilever_hole'
        vF = -vF;        
        % two-scale            
        nodes(:,4) = 1:size(nodes,1);
        [ix,~] = find(abs(nodes(:,1)-maxx)<1e-5);
        nodes1 = nodes(ix, :);
        [iy,~] = find(abs(nodes1(:,2)-miny)<1e-5);
        nodes2 = nodes1(iy, :);
        loadnid = nodes2(:, 4);
        loaddofs = 3*loadnid(:) - 1;

        [ix,~] = find(abs(nodes(:,1)-minx)<1e-5);
        fixednid = nodes(ix, 4);
        fixeddofs = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';            

    case 'Lshape'
        vF = -vF;        
        % two-scale            
        nodes(:,4) = 1:size(nodes,1);
        [ix,~] = find(abs(nodes(:,1)-maxx)<1e-5);
        nodes1 = nodes(ix, :);
        [iy,~] = find(abs(nodes1(:,2)-miny)<1e-5);
        nodes2 = nodes1(iy, :);
        loadnid = nodes2(:, 4);
        loaddofs = 3*loadnid(:) - 1;

        [ix,~] = find(abs(nodes(:,1)-minx)<1e-5);
        fixednid = nodes(ix, 4);
        fixeddofs = [3*fixednid(:), 3*fixednid(:)-1, 3*fixednid(:)-2]';       
    end
    
    if optChanged == 1
        vF = vF * 7 / length(loadnid);    
    end

    alldofs = size(nodes, 1) * 3;
    
    fixeddofs = fixeddofs(:);
    freedofs = setdiff(1:alldofs, fixeddofs);
    fext = sparse(loaddofs(:), 1, vF(:), alldofs, 1);

%     figure; scatter3(nodes(:,1),nodes(:,2),nodes(:,3)); axis equal; hold on
%     scatter3(nodes(fixednid(:),1),nodes(fixednid(:),2),nodes(fixednid(:),3), 'filled'); axis equal; hold on
% %     scatter3(nodes(freedofs(3:3:end)/3,1),nodes(freedofs(3:3:end)/3,2),nodes(freedofs(3:3:end)/3,3), 'filled'); axis equal; hold on
% %     scatter3(nodes(fixednid2(:),1),nodes(fixednid2(:),2),nodes(fixednid2(:),3), 'filled'); axis equal; hold on
% %     scatter3(nodes(fixednid3(:),1),nodes(fixednid3(:),2),nodes(fixednid3(:),3), 'filled'); axis equal; hold on
%     scatter3(nodes(loadnid,1),nodes(loadnid,2),nodes(loadnid,3), 'filled'); axis equal; hold on
end

function [loaddofs, loadnid, vF] = stretch_force(vF0, cx, maxy, maxz, dx, nodes)

    idx = []; vF = [];
    
    for cy = 0:dx:maxy
        for cz = 0:dx:maxz
            [idx_tmp, vF_tmp] = project_force(cx, cy, cz, vF0, nodes);
            idx = [idx; idx_tmp];
            vF  = [vF; vF_tmp];
        end
    end

    loadnid = idx;
    loaddofs = [3*loadnid-2];
end

% three bars at (deltax, :, maxz), (halfx, :, 0), (maxx-deltax, :, maxz) mimicking contact forces
% only located on that macro-nodes
function [loaddofs, loadnid, vF] = contact_F(vF0, nelx, nely, nelz, num_b, lx, offset, nodes)

    maxx = lx * nelx;
    maxy = lx * nely;
    maxz = lx * nelz;

    % three bars mimicking contact forces
    % 1. (halfx, maxy, :) - (0,-vf,0)
    fx = maxx/2; fy = maxy; fz = -1;
    [idx1, vF1] = project_force(fx, fy, fz, -vF0, nodes);

    % 2. (deltax, 0, :) - (0,vf/2,0)
    fx = offset; fy = 0; fz = -1;
    [idx2, vF2] = project_force(fx, fy, fz, vF0/2, nodes);

    fx = maxx - offset; fy = 0; fz = -1;
    [idx3, vF3] = project_force(fx, fy, fz, vF0/2, nodes);

    loadnid = [idx1(:); idx2(:); idx3(:)];
    loaddofs = 3 * loadnid - 1;
    vF = [vF1; vF2; vF3];
end

% three bars at (deltax, :, maxz), (halfx, :, 0), (maxx-deltax, :, maxz) mimicking contact forces
% only located on that macro-nodes
function [loaddofs, loadnid, vF] = mimicking_contact_F(vF0, nelx, nely, nelz, num_b, lx, offset, nodes)

    maxx = lx * nelx;
    maxy = lx * nely;
    maxz = lx * nelz;

    % dx = lx / microx;   
    step = lx / (num_b + 1);

    % three bars mimicking contact forces
    % 1. (halfx, maxy, :) - (0,-vf,0)
    idx1 = []; vF1 = [];
    fx = maxx/2; fy = maxy; fz = -1;
    for i = max(fx-offset, 0) : step : min(fx+offset, nelx+1)
        for k = max(fy-offset, 0) : step : min(fy+offset, nely+1)

            % update vF
            dist = sqrt((i-fx)^2 + (k-fy)^2);
            if dist < offset
                vF = vF0 * (1 - dist / offset);
                [idx_tmp, vF_tmp] = project_force(i, k, fz, -vF, nodes);

                idx1 = [idx1; idx_tmp];
                vF1  = [vF1;  vF_tmp];
            end
        end
    end

    % 2. (deltax, 0, :) - (0,vf/2,0)
    idx2 = []; vF2 = [];
    fx = offset; fy = 0; fz = -1;
    for i = max(fx-offset, 0) : step : min(fx+offset, nelx+1)
        for k = max(fy-offset, 0) : step : min(fy+offset, nely+1)
            % update vF
            dist = sqrt((i-fx)^2 + (k-fy)^2);
            if dist < offset
                vF = vF0/2 * (1 - dist / offset);
                [idx_tmp, vF_tmp] = project_force(i, k, fz, vF, nodes);

                idx2 = [idx2; idx_tmp];
                vF2  = [vF2;  vF_tmp];
            end
        end
    end

    % 3. (maxx-deltax, 0, :) - (0,vf/2,0)
    idx3 = []; vF3 = [];
    fx = maxx - offset; fy = 0; fz = -1;
    for i = max(fx-offset, 0) : step : min(fx+offset, nelx+1)
        for k = max(fy-offset, 0) : step : min(fy+offset, nely+1)
            % update vF
            dist = sqrt((i-fx)^2 + (k-fy)^2);
            if dist < offset
                vF = vF0/2 * (1 - dist / offset);
                [idx_tmp, vF_tmp] = project_force(i, k, fz, vF, nodes);

                idx3 = [idx3; idx_tmp];
                vF3  = [vF3;  vF_tmp];
            end
        end
    end

    loadnid = [idx1(:); idx2(:); idx3(:)];
    loaddofs = 3 * loadnid - 1;
    vF = [vF1; vF2; vF3];
end

function [loaddofs, loadnid, vF] = twisting_force_face_2scale(nelx, nely, nelz, num_b, microx, vF0, nodes, lx)

    globalx = nelx * microx;
    globaly = nely * microx;
    globalz = nelz * microx;

    new_nely = nely * (num_b + 1);
    new_nelz = nelz * (num_b + 1);

    maxx = nelx * lx;
    midy = nely * lx / 2;
    midz = nelz * lx / 2;

    [ix, ~] = find(nodes(:,1)==maxx);
    loadnid = nodes(ix, 4);
    loaddofs = [3*loadnid - 1, 3 * loadnid];

    % mangnitude to be same as full-scale
    vF0 = vF0 * (globaly + 1) * (globalz + 1) / (new_nely + 1) / (new_nelz + 1);

    cy = nodes(ix, 2) - midy;
    cz = nodes(ix, 3) - midz;
    cc = cy.^2 + cz.^2;
    cosa = sqrt(cz.^2 ./ cc);
    sina = sqrt(cy.^2 ./ cc);
    
    cosa(cc==0) = 0;
    sina(cc==0) = 0;
        
    cz_positive = double(cz > 0);
    cz_positive(cz < 0) = -1;
    cy_positive = double(cy > 0);
    cy_positive(cy < 0) = -1;

    vF_y0 = vF0 * cosa;
    vF_z0 = vF0 * sina;

    vF_y = vF_y0 .* (-cz_positive);
    vF_z = vF_z0 .* (cy_positive);
    vF = [vF_y(:), vF_z(:)];            
end

function [loaddofs, loadnid, vF] = twisting_force_face(globalx, globaly, globalz, vF0)

    midy = round(globaly / 2);
    midz = round(globalz / 2);

    nnodes = (1) *(globaly+1) *(globalz+1);
    loaddofs = zeros(nnodes, 2);
    vF = zeros(nnodes, 2);
    loadnid = zeros(nnodes, 1);

    for i = globalx : globalx
        for j = 0 : globaly
            for k = 0 : globalz
                vy = (globaly-j)-midy;
                vz = k-midz;
                vv = vy^2 + vz^2;
                cosa = sqrt(vz^2 / vv);
                sina = sqrt(vy^2 / vv);

                vF_y0 = vF0 * cosa;
                vF_z0 = vF0 * sina;
                
                if vz > 0
                    vF_y = -vF_y0;
                elseif vz < 0
                    vF_y = vF_y0;
                else
                    vF_y = 0;
                end

                if vy > 0
                    vF_z = vF_z0;
                elseif vy < 0
                    vF_z = -vF_z0;
                else
                    vF_z = 0;
                end
                
                idx = j + (i-globalx) * (globaly+1) + k * 1 * (globaly+1) + 1;
                loadnid0 = j + i * (globaly+1) + k * (globalx+1) * (globaly+1) + 1;
                loaddofs(idx, :) = [3 * loadnid0-1, 3 * loadnid0];
                vF(idx, :) = [vF_y, vF_z];
                
                loadnid(idx) = loadnid0;
            end
        end
    end
    % idx = loaddofs>0;
    % loaddofs = loaddofs(idx);
    % vF = vF(idx);
    % loadnid = loadnid(loadnid>0);
end

function [loaddofs, loadnid, vF] = twisting_force_body(globalx, globaly, globalz, vF0)

    midy = round(globaly / 2);
    midz = round(globalz / 2);

    nnodes = (globalx) *(globaly+1) *(globalz+1);
    loaddofs = zeros(nnodes, 2);
    vF = zeros(nnodes, 2);
    loadnid = zeros(nnodes, 1);

    for i = 1 : globalx
        for j = 0 : globaly
            for k = 0 : globalz
                vy = (globaly-j)-midy;
                vz = k-midz;
                vv = vy^2 + vz^2;
                cosa = sqrt(vz^2 / vv);
                sina = sqrt(vy^2 / vv);

                vF_y0 = vF0 * cosa;
                vF_z0 = vF0 * sina;
                
                if vz > 0
                    vF_y = -vF_y0;
                elseif vz < 0
                    vF_y = vF_y0;
                else
                    vF_y = 0;
                end

                if vy > 0
                    vF_z = vF_z0;
                elseif vy < 0
                    vF_z = -vF_z0;
                else
                    vF_z = 0;
                end
                
                idx = j + (i-1) * (globaly+1) + k * globalx * (globaly+1) + 1;
                loadnid0 = j + i * (globaly+1) + k * (globalx+1) * (globaly+1) + 1;
                loaddofs(idx, :) = [3 * loadnid0-1, 3 * loadnid0];
                vF(idx, :) = [vF_y, vF_z];
                
                loadnid(idx) = loadnid0;
            end
        end
    end
    % idx = loaddofs>0;
    % loaddofs = loaddofs(idx);
    % vF = vF(idx);
    % loadnid = loadnid(loadnid>0);
end

% apply force on nodes coordinates at (vx,vy,vz) = vF
% if vx / vy / vz < 0, means taking all indices
function [loadnid, vF3] = project_force(vx, vy, vz, vF, nodes)
    if vx > -1e-3 % valid
        v = min(abs(nodes(:,1) - vx));
        [ix, ~] = find(abs(nodes(:,1)-v-vx) < 1e-3);
        nodes1 = nodes(ix, :);
    else
        nodes1 = nodes;
    end

    if vz > -1e-3 % valid
        v = min(abs(nodes1(:,3) - vz));
        [iz, ~] = find(abs(nodes1(:,3)-v-vz) < 1e-3);
        nodes2 = nodes1(iz, :);
    else
        nodes2 = nodes1;
    end

    if vy > -1e-3 % valid
        v = min(abs(nodes2(:,2) - vy));
        [iy, ~] = find(abs(nodes2(:,2)-v-vy) < 1e-3);
        loadnid = nodes2(iy, 4);
    else
        loadnid = nodes2(:, 4);
    end

    vF3 = repmat(vF, length(loadnid), 1);
end

function F = neighbors_F(f_i, f_j ,f_k, fv, step, nelx, nely, nelz)

    nodes_num = (nelx+1)*(nely+1)*(nelz+1);
    F = zeros(nodes_num, 3);
    numj = length(f_j);
    
    for i = max(f_i-step, 1) : min(f_i+step, nelx+1)
        for k = max(f_k-step, 1) : min(f_k+step, nelz+1)

            dist = sqrt((i-f_i)^2 + (k-f_k)^2);
            if dist > step
                continue;
            else
                fz = dist / step * fv/2; % new fv
            
                nid = f_j + (i-1) * (nely+1) + (k-1)*(nely+1)*(nelx+1);            
                F(nid, :) = repmat([0, 0, fz], numj, 1);                
            end
        end
    end
end