%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2022-01-18 00:12:35
 % @LastEditTime: 2022-01-26 20:25:02
%--------------------------
maxloop = 200;    % Maximum number of iterations
tolx = 0.001;      % Terminarion criterion
displayflag = 0;  % Display structure flag
% USER-DEFINED MATERIAL PROPERTIES
E0 = 1;           % Young's modulus of solid material
Emin = 1e-9;      % Young's modulus of void-like material
nu = 0.3;         % Poisson's ratio
penal = 1;
rmin = 2;
volfrac = 0.4;

[nodes, tets] = read_tet();
% figure; scatter3(nodes(:,1), nodes(:,2), nodes(:,3));

maxx = max(nodes(:, 1)); minx = min(nodes(:, 1));
maxy = max(nodes(:, 2)); miny = min(nodes(:, 2));
maxz = max(nodes(:, 3)); minz = min(nodes(:, 3));

optDesign = 'cantilever'; vF = 1;
[freedofs, fixeddofs, F, alldofs] = design_domain(optDesign, maxx, minx, ...
    maxy, miny, maxz, minz, vF, nodes);

% PREPARE FINITE ELEMENT ANALYSIS
nele = size(tets, 1);
U = zeros(alldofs,1);

edofMat0 = [3*tets-2, 3*tets-1, 3*tets]; % 12 cols [x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4]
edofMat  = edofMat0(:, [1,5,9,2,6,10,3,7,11,4,8,12]);

iK = reshape(kron(edofMat,ones(12,1))',12*12*nele,1);
jK = reshape(kron(edofMat,ones(1,12))',12*12*nele,1);

[sK0, vol] = intKE_tet(nodes, tets, 1, nu);
vol0 = sum(vol);

% INITIALIZE ITERATION
x = repmat(volfrac,[nele, 1]);
xPhys = x; 
loop = 0; 
change = 1;

m     = 1;                % The number of general constraints. 绾︽潫鏉′欢涓?暟
n     = nele;      % The number of design variables x_j. 璁捐?鍙橀噺x涓?暟nelx * nely
xmin  = zeros(n,1);       % Column vector with the lower bounds for the variables x_j. x鐨勪笅鐣岀殑鍒楀悜閲?
xmax  = ones(n,1);        % Column vector with the upper bounds for the variables x_j. x鐨勪笂鐣岀殑鍒楀悜閲?
xold1 = x(:);             % xval, one iteration ago (provided that iter>1). 涓?釜杩?唬涔嬪墠鐨剎val
xold2 = x(:);             % xval, two iterations ago (provided that iter>2). 涓や釜杩?唬涔嬪墠鐨剎val
low   = ones(n,1);        % Column vector with the lower asymptotes from the previous iteration (provided that iter>1). 涓婁竴涓?凯浠ｇ殑涓嬫笎杩戠嚎鐨勫垪鍚戦噺
upp   = ones(n,1);        % Column vector with the upper asymptotes from the previous iteration (provided that iter>1). 涓婁竴涓?凯浠ｇ殑涓婃笎杩戠嚎鐨勫垪鍚戦噺
a0    = 1;                % The constants a_0 in the term a_0*z.
a     = zeros(m,1);       % Column vector with the constants a_i in the terms a_i*z.
c_MMA = 1000*ones(m,1);   % Column vector with the constants c_i in the terms c_i*y_i.
d     = zeros(m,1);       % Column vector with the constants d_i in the terms 0.5*d_i*(y_i)^2.

% START ITERATION
while change > tolx && loop < maxloop
    loop = loop+1;
    % FE-ANALYSIS
    sK = sK0 .* (Emin+xPhys(:)'.^penal*(E0-Emin));
    K = sparse(iK,jK,sK); K = (K+K')/2;
    U(freedofs,:) = K(freedofs,freedofs)\F(freedofs,:);

    % OBJECTIVE FUNCTION AND SENSITIVITY ANALYSIS
    U1(1, :, :) = U(edofMat)'; % [1,6,n]
    U2(:, 1, :) = U(edofMat)'; % [6,1,n]
    sK1 = sK0 .* (penal*(E0-Emin)*xPhys'.^(penal-1)); % [36,m]
    sK2 = reshape(sK1, 12, 12, []);
    dc = mtimesx(mtimesx(U1, sK2), U2);
    dc = squeeze(dc);
    
    c = F' * U;

    defn = reshape(U, 3, [])' + nodes;
%     figure; scatter3(defn(:,1), defn(:,2), defn(:,3));
    DT = triangulation(tets,defn);
    figure; tetramesh(DT,'FaceAlpha',0.3);

    v = sum(vol .* xPhys(:));
    dv = vol;
    
    %% MMA
    xval  = x(:);
    f0val = c;
    df0dx = - dc(:);   
    fval  = v / (volfrac*vol0) - 1;
    dfdx  = dv(:)';
    [xnew, ~, ~, ~, ~, ~, ~, ~, ~, low,upp] = mmasub(m, n, loop, xval, ...
    xmin, xmax, xold1, xold2, f0val,df0dx,fval,dfdx,low,upp,a0,a,c_MMA,d);

    % Update MMA Variables
    xPhys    = xnew(:);
    xold2    = xold1(:); xold1    = x(:);

    change = max(abs(xnew(:)-x(:)));
    x = xnew;
    % PRINT RESULTS
    fprintf(' It.:%5i Obj.:%11.4f Vol.:%7.3f ch.:%7.3f\n',loop,c,mean(xPhys(:)),change);
end

function [nodes, valid_tets] = read_tet()
    datapath = "D:\\project\\opt-cvt\\3D-code\\3d-opt\\data\\mesh-coarse\\tet98.txt";
    fileID = fopen(datapath, 'r');
    info = fscanf(fileID, '%f');

    npnts  = info(1);
    ntets  = info(2); 

    nodes = reshape(info(3 : 3 + npnts*3 - 1), 3, [])'; % all nodes coodinates of cdt
    tets  = reshape(info(3 + npnts*3 : end), 4, [])' + 1;

    fclose(fileID);

    [valid_tets] = checkValid_tetmesh(nodes, tets);
end