% This file generates an optimized design
% Triangular geometry
close all;
clear;
clc;
commandwindow; 
%% Material, load, setup
E = 30e6; nu = 0.2; 
massden = 25; % [kN/m^3] 
loadmag = 5; % External load [kN/m^2]                        
penalmax = 1; penal = 1; % For SIMP on beam width
defallow = 0.015; % Allowed deflection
% Choose problem type (trimmed triangular domain)
type = 'trimmedTri';
% Limits for physical design variables
b_max = 0.12; b_min = 1e-3;
H_max = 0.4; th_min = 0.07; h_max = H_max - th_min; 
h_min = 1e-3; th_max = H_max - h_min;
% Initial design
th = 0.1; h_beam = 0.2; b_beam = 0.1; 
% MMA move limits
moveSize = 0.2;
moveShape = 0.05;
% Smoothing
nLap = 10;
%% Generate the triangular mesh
% Size of trimmed triangle 
lengthOut = 12;
lengthIn = 4;
resol = 24;
dx = lengthOut/resol;
dy = tan(pi/3)*dx/2;
pntsOut = lengthOut/dx + 1;
pntsIn = lengthIn/dx + 1;
pntsEdge = (lengthOut-lengthIn)/dx +1 ;
% Prepare arrays for boundary nodes
innerBound = zeros(pntsIn,1);
outerBound = zeros(pntsOut,1);
edge1Bound = (1:pntsEdge)';
edge2Bound = zeros(pntsEdge,1);
% First generate nodes in lower parallelogram
nodes = zeros(1000,2);
nrows1 = lengthIn/dx;
k = 0;
cnt1 = 1; cnt2 = 1;
for i = 1:nrows1 % row number
    npnts = (lengthOut-lengthIn)/dx + 1;
    X0 = lengthIn - (i-1)*dx/2;
    XN = X0 + lengthOut - lengthIn;
    Y = (i-1)*dy;
    pnts = linspace(X0,XN,npnts);
    nodes(k+1:k+npnts,1) = pnts;
    nodes(k+1:k+npnts,2) = Y;
    % Register boundaries
    innerBound(cnt1,1) = k+1;
    outerBound(cnt2,1) = k+npnts;
    cnt1 = cnt1 + 1;
    cnt2 = cnt2 + 1;
    k = k + npnts;
end
innerBound(cnt1,1) = k+1;
% Now generate nodes in upper triangle
cnt1 = 1;
for i = nrows1+1:(resol+1) % row number
    npnts = resol+2-i;
    X0 = (i-1)*dx/2;
    XN = lengthOut-X0;
    Y = (i-1)*dy;
    pnts = linspace(X0,XN,npnts);
    nodes(k+1:k+npnts,1) = pnts;
    nodes(k+1:k+npnts,2) = Y;
    % Register boundaries
    edge2Bound(cnt1,1) = k+1;
    outerBound(cnt2,1) = k+npnts;
    cnt1 = cnt1 + 1;
    cnt2 = cnt2 + 1;
    k = k + npnts;
end
nnodes = k;
nodes(k+1:end,:) = [];
allBounds = union(innerBound,union(outerBound,union(edge1Bound,edge2Bound)));
% Loop over triangles in lower parallelogram
ntri = 1000;
plates = zeros(ntri,3);
k = 1;
for i = 1:nrows1 
    for j = 1:pntsEdge-1
        % upside down triangle
        n1 = (i-1)*pntsEdge+ j;
        n2 = n1 + pntsEdge + 1;
        n3 = n2 - 1;
        plates(k,1:3) = [n1,n2,n3];
        % regular triangle
        n3 = n2;
        n2 = n1 + 1;
        plates(k+1,1:3) = [n1,n2,n3];
        k = k + 2;
    end
end
cornerNode =  pntsEdge*nrows1 + 1;
% Now generate triangles in upper triangle
for i = nrows1+1:resol+1 % row number
    npnts = resol+2-i;
    for j = 1:npnts-1
        n1 = cornerNode + j - 1;
        n2 = n1 + 1;
        n3 = n1 + npnts;
        plates(k,1:3) = [n1,n2,n3];
        k = k + 1;
    end
    cornerNode = n2 + 1;
end
cornerNode =  pntsEdge*(nrows1+1) + 1;
for i = nrows1+2:resol % row number
    npnts = resol+2-i;
    for j = 1:npnts-1
        n1 = cornerNode + j - 1;
        n2 = n1 - npnts;
        n3 = n1 + 1;
        plates(k,1:3) = [n1,n2,n3];
        k = k + 1;
    end
    cornerNode = n3 + 1;
end
plates(k:end,:) = [];
%% [OPTIONAL] Create a pde mesh and view
model = createpde();
geometryFromMesh(model,nodes',plates');
figure(1);
pdeplot(model,NodeLabels='on');
axis equal tight
%% Prepare smoothing operator
% Find connectivities
[conn,connnum,count] = neighborelem(plates,max(max(plates))); 
k = 0;
Lap = zeros(nnodes);
while k < nnodes
    k=k+1;
    % Find vertex neighborhood
    indt01=conn{k}; % Element indices
    indv01 = plates(indt01,:);
    indv01 = unique(indv01(:));
    indv01 = setdiff(indv01,k);
    Lap(k,indv01) = 1./size(indv01,1);
end
%% Find active DOFs (will move)
activeDOFs = ones(nnodes*2,1);
% All bounds don't move
activeDOFs(2*allBounds-1,1) = 0;
activeDOFs(2*allBounds,1) = 0;
nactiveDOFs = sum(activeDOFs);
% Prepare also node-based
kx = size(allBounds,1);
ky = size(allBounds,1);
passiveNodeX = allBounds;
passiveNodeY = allBounds;
activeNodeX = setdiff(1:nnodes,passiveNodeX);
activeNodeY = setdiff(1:nnodes,passiveNodeY);
LapX = Lap; LapY = Lap;
LapX(passiveNodeX,activeNodeX) = 0;
LapX(passiveNodeX,passiveNodeX) = eye(kx);
LapY(passiveNodeY,activeNodeY) = 0;
LapY(passiveNodeY,passiveNodeY) = eye(ky);
% Create single operator
LapXn = LapX^nLap; 
LapYn = LapY^nLap; 
% Transpose for sensitivities
LapTX = LapX'^nLap;
LapTY = LapY'^nLap;
%% Generate data for beams
% Create list of edges
nplates = size(plates,1);
nedges = nplates*3; % Number of non-unique edges
edges = zeros(nedges,2);
for i = 1:nplates
    edges(3*i-2,1:2) = [plates(i,1),plates(i,2)];
    edges(3*i-1,1:2) = [plates(i,2),plates(i,3)];
    edges(3*i,1:2) = [plates(i,3),plates(i,1)];
end
% Organize such that n1<n2
for i = 1:nedges
    if edges(i,1) > edges(i,2)
        tmp = edges(i,1);
        edges(i,1) = edges(i,2);
        edges(i,2) = tmp;
    end
end
edges_srt = sortrows(edges);
edges = unique(edges_srt,'rows');
nedges = size(edges,1);
nbeams = nedges;
%% Apply BC
ndof = 6*nnodes;
supdofs = applyBCtri(innerBound,outerBound,edge1Bound,edge2Bound);
freedofs = setdiff(1:ndof,supdofs);
%% Optimization initialization
ndv1 = nbeams + 2; % b, h, th
ndv2 = nactiveDOFs; % X and Y
ndv = ndv1 + ndv2;
% Initial design
b_list = ones(nbeams,1)*b_beam;
% Initialize design variables
x(1:nbeams,1) = (b_list - b_min)/(b_max-b_min);
x(ndv1-1,1) = (h_beam - h_min)/(h_max-h_min);
x(ndv1,1) = (th - th_min)/(th_max-th_min);
b_list_p = b_min + (b_max-b_min)*x(1:nbeams,1).^penal;
xtmp = nodes(:,1);
ytmp = nodes(:,2);
xytmp = [xtmp ytmp]'; xytmp = xytmp(:);
xytmp(activeDOFs==0) = [];
x(ndv1+1:ndv) = xytmp;
opttol = 1e-3;
maxiter = 100;
stats = zeros(maxiter,4);
nodesInit = nodes;
%% MMA
m     = 2;                  % The number of general constraints.
xmin  = zeros(ndv,1);       % Column vector with the lower bounds for the variables x_j.
xmax  = ones(ndv,1);        % Column vector with the upper bounds for the variables x_j.
xold1 = x;                  % xval, one iteration ago (provided that iter>1).
xold2 = x;                  % xval, two iterations ago (provided that iter>2).
low   = 0*ones(ndv,1);      % Column vector with the lower asymptotes from the previous iteration (provided that iter>1).
upp   = ones(ndv,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 = [1e3;1e3];          % 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.
%% [OPTIONAL] Videos
% v1 = VideoWriter('design_ex4.avi');
% v2 = VideoWriter('mesh_ex4.avi');
% v1.FrameRate = 4;
% v2.FrameRate = 4;
% open(v1);
% open(v2);
% close(v1);
% close(v2);
%% Start iterations
for iter = 1:maxiter
    % Continuation
    if (mod(iter,25)==0)
        penal = min(penal+1,penalmax);
        display(penal);
    end
    % Compute volume and sensitivities
    V = 0;
    dv1 = zeros(ndv1,1);
    dv2 = zeros(2*nnodes,1);
    % Volume of beams
    [V,dv1,dv2] = volbeam(V,dv1,dv2,nodes,edges,b_list,h_beam);
    dv2(2*activeNodeX-1,1) = LapTX(activeNodeX,activeNodeX)*dv2(2*activeNodeX-1,1);
    dv2(2*activeNodeY,1) = LapTY(activeNodeY,activeNodeY)*dv2(2*activeNodeY,1);
    dv2(activeDOFs==0) = [];
    % Volume of plates
    [totalArea,plates] = areaplate(nodes,plates);
    V = V + totalArea*th;
    dv1(ndv1,1) = totalArea;
    % Assemble external load: distributed in Z
    Fext = zeros(ndof,1);
    for i = 1:nplates
        n1 = plates(i,1); n2 = plates(i,2); n3 = plates(i,3);
        loaddof = [6*n1-3 6*n2-3 6*n3-3];
        Fext(loaddof,1) = Fext(loaddof,1) - loadmag*plates(i,4)/3;
    end
    % Assemble self weight load
    Fsw = 0*Fext;
    [Fsw,dFswdth] = assmFplate(Fsw,nodes,plates,th,massden);
    [Fsw,dFswdh] = assmFbeam(Fsw,nodes,edges,b_list_p,h_beam,massden);
    % Assemble plate-beam stiffness
    K = zeros(ndof);
    K = assmKplate(K,nodes,plates,E,nu,th);
    K = assmKbeam(K,nodes,edges,E,nu,b_list_p,h_beam,th);
    % Solve
    F = Fext + Fsw;
    U = 0*F;
    U(freedofs,1) = K(freedofs,freedofs) \ F(freedofs,1);
    comp = F'*U;
    % Compute maximum deflection
    Uver = U(3:6:end,1);
    maxUver = max(-Uver);
    pN = 16;
    apprxmaxUver = (sum(Uver.^pN))^(1/pN);
    %% Sensitivity analysis
    % Adjoint
	coeff = (1/pN)*(sum(Uver.^pN))^(1/pN-1);
    inner = pN*(Uver.^(pN-1));
    adjRHS = 0*U;
    adjRHS(3:6:end,1) = -coeff*inner;
    ADJ = 0*U;
    ADJ(freedofs,1) = K(freedofs,freedofs) \ adjRHS(freedofs,1);
	% Deflection sensitivity analysis: b, h, th
	dd1 = zeros(ndv1,1);
    % uKu terms
    dd1 = ddefbeam(dd1,U,ADJ,nodes,edges,E,nu,b_list_p,h_beam,th);
    dd1 = ddefplate(dd1,U,ADJ,nodes,plates,E,nu,th);
    % fu terms (only self weight depends on sizes)
    dd1(ndv1-1,1) = dd1(ndv1-1,1) - dFswdh'*ADJ;
    dd1(ndv1,1) = dd1(ndv1,1) - dFswdth'*ADJ;
    dFswdbADJ = assmdFswdbU(ADJ,nodes,edges,h_beam,massden);
    dd1(1:nbeams,1) = dd1(1:nbeams,1) - dFswdbADJ;
	% Deflection sensitivity analysis: nodes
    dd2 = zeros(2*nnodes,1);
    % uKu terms
    dd2 = ddefbeam_nodes(dd2,U,ADJ,nodes,edges,E,nu,b_list_p,h_beam,th);
    dd2 = ddefplate_nodes(dd2,U,ADJ,nodes,plates,E,nu,th);
    % f*u terms (external and SW): plates only 
    dFdXADJ = assmdFdXU(ADJ,nodes,plates,loadmag,th,massden);
    dd2 = dd2 - dFdXADJ;
    % fsw*u terms: beams
    dFdXADJ = assmdFdXU_beams(ADJ,nodes,edges,b_list_p,h_beam,massden);
    dd2 = dd2 - dFdXADJ;   
    dd2(2*activeNodeX-1,1) = LapTX(activeNodeX,activeNodeX)*dd2(2*activeNodeX-1,1);
    dd2(2*activeNodeY,1) = LapTY(activeNodeY,activeNodeY)*dd2(2*activeNodeY,1);
    dd2(activeDOFs==0) = [];
    
    %% Solve min V s.t. deflection
    f0val = V;
    df0dx = zeros(ndv,1);
    df0dx(1:nbeams,1) = dv1(1:nbeams,1)*(b_max-b_min);
    df0dx(ndv1-1,1) = dv1(ndv1-1,1)*(h_max-h_min);
    df0dx(ndv1,1) = dv1(ndv1,1)*(th_max-th_min);
    df0dx(ndv1+1:ndv,1) = dv2;
    fval(1,1) = maxUver/defallow - 1;
    dfdx = zeros(m,ndv);
    dfdx(1,1:nbeams) = 1/defallow*(b_max-b_min)*penal*(dd1(1:nbeams,1).*x(1:nbeams,1).^(penal-1))';
    dfdx(1,ndv1-1) = 1/defallow*dd1(ndv1-1,1)*(h_max-h_min);
    dfdx(1,ndv1) = 1/defallow*dd1(ndv1,1)*(th_max-th_min);
    dfdx(1,ndv1+1:ndv) = 1/defallow*dd2';
    fval(2,1) = (h_beam + th)/H_max - 1;
    dfdx(2,1:ndv1) = [zeros(1,nbeams) (h_max-h_min)/H_max (th_max-th_min)/H_max];
    dfdx(2,ndv1+1:ndv) = 0;
    % MMA update of topology and sizing
    xmin(1:ndv1,1) = max(0,x(1:ndv1,1)-moveSize);
    xmax(1:ndv1,1) = min(1,x(1:ndv1,1)+moveSize);
    [xmma1, ~, ~, ~, ~, ~, ~, ~, ~, low1,upp1] = ...
        mmasub(m,ndv1,iter,x(1:ndv1,1),xmin(1:ndv1,1),xmax(1:ndv1,1),...
        xold1(1:ndv1,1),xold2(1:ndv1,1),comp,df0dx(1:ndv1,1),fval,dfdx(:,1:ndv1),...
        low(1:ndv1,1),upp(1:ndv1,1),a0,a,c_MMA,d);
    % MMA update of shape
    xmin(ndv1+1:ndv,1) = x(ndv1+1:ndv,1)-moveShape;
    xmax(ndv1+1:ndv,1) = x(ndv1+1:ndv,1)+moveShape;
    [xmma2, ~, ~, ~, ~, ~, ~, ~, ~, low2,upp2] = ...
        mmasub(m,ndv2,iter,x(ndv1+1:ndv,1),xmin(ndv1+1:ndv,1),xmax(ndv1+1:ndv,1),...
        xold1(ndv1+1:ndv,1),xold2(ndv1+1:ndv,1),comp,df0dx(ndv1+1:ndv,1),fval,dfdx(:,ndv1+1:ndv),...
        low(ndv1+1:ndv,1),upp(ndv1+1:ndv,1),a0,a,c_MMA,d);
    xmma = [xmma1;xmma2];
    low = [low1;low2];
    upp = [upp1;upp2];
    % Update MMA Variables
    change = max(abs(x(1:ndv1,1)-xmma1));
    xold2 = xold1;
    xold1 = x;
    x = xmma;
    
    %% Finalize    
    % Save stats
    mnd = xold1(1:nbeams,1)'*(1-xold1(1:nbeams,1));
    stats(iter,1:4) = [f0val fval(1,1) fval(2,1) mnd];
    % Update physical variables
    b_list = b_min + (b_max-b_min)*x(1:nbeams,1);
    b_list_p = b_min + (b_max-b_min)*x(1:nbeams,1).^penal;
    h_beam = h_min + x(ndv1-1,1)*(h_max-h_min);
    th = th_min + x(ndv1,1)*(th_max-th_min);
     % Update nodes
    oldNodes = nodes'; oldNodes = oldNodes(:);
    newNodes = oldNodes;
    newXYcoor = x(ndv1+1:end,1);
    newNodes(activeDOFs==1) = newXYcoor(:);
    nodes = reshape(newNodes,2,nnodes)';
    % Smooth nodes
    x_in = nodes(:,1);
    y_in = nodes(:,2);
    x_out = LapXn*x_in;
    y_out = LapYn*y_in;
    nodes(:,1) = x_out;
    nodes(:,2) = y_out;
    % Draw
    figure(1); clf;
    plot_optimized_design_std;
    % frame = getframe(gcf);
    % writeVideo(v1,frame);
    figure(2); clf;
    plot_mesh;
    % frame = getframe(gcf);
    % writeVideo(v2,frame);
    % Display data
    fprintf(' ITER: %3i OBJ: %6.3e CONST: %6.3e %6.3e CH: %4.3f \n',...
        iter,f0val,fval(1,1),fval(2,1),change);
    if (change<opttol); break; end
end
