% This file generates a reference case
% Trapezoidal geometry
close all;
clear;
clc;
commandwindow; 
%% Generate the triangular mesh

% Size of trimmed triangle 
lengthOut = 12;
lengthIn = 4;
resol = 12;
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,:) = [];

% Create a pde mesh and view
model = createpde();
geometryFromMesh(model,nodes',plates');
figure(1);
pdeplot(model,NodeLabels='on');
axis equal tight

% % Find connectivities
% [conn,connnum,count] = neighborelem(plates,max(max(plates)));
%  
% nLap = 5;
% 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
% LapT = Lap'.^nLap;

% Compute areas of triangles
[~,plates] = areaplate(nodes,plates);

%% Material, load, setup
E = 30e6; nu = 0.2; massden = 25; 
loadmag = 5; 
defallow = 0.016;
type = 'trimmedTri'; % '4corners_fixed_SYM';%'simply_supp_SYM';% 'simply_supp_SYM'; %'SSSYM'; %'rect4corners'; 'SSSYM'
%% 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 = nnodes*2; % X and Y
ndv = ndv1 + ndv2;
% Initial design
h_total = 0.42; % Total thickness
th = 0.07; % Plate thickness 
h_beam = h_total-th; % Beam height 
b_beam = 0.12;
b_list = ones(nbeams,1)*b_beam;

% th = 0.1; h_beam = 0.2; b_beam = 0.1; 
% b_list = ones(nbeams,1)*b_beam;
% % Limits for physical design variables
% b_max = 0.2; 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;
% % 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.
% % %% 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);

%% Computations
% Volume of beams
V = 0;
dv1 = zeros(ndv1,1);
dv2 = zeros(2*nnodes,1);
[V,~,~] = volbeam(V,dv1,dv2,nodes,edges,b_list,h_beam);
% Volume of plates
[totalArea,plates] = areaplate(nodes,plates);
V = V + totalArea*th;
% 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,~] = assmFplate(Fsw,nodes,plates,th,massden);
[Fsw,~] = assmFbeam(Fsw,nodes,edges,b_list,h_beam,massden);
% Assemble ribbed plate stiffness
K = zeros(ndof);
K = assmKplate(K,nodes,plates,E,nu,th);
K = assmKbeam(K,nodes,edges,E,nu,b_list,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);
% Draw
figure(2)
msh = model.Mesh;
pdeplot(msh,XYData=Uver,ZData=Uver,Mesh="off");
axis equal tight off
view(2)