% Matlab script to run trussFEA code
% Written for MN 203 in January 2020 (G. K. Ananthasuresh)
%
close all; clear all; clc; hold off; % Housekeeping commands
% Load data and save into appropriate arrays
load node.dat
NNODE = size(node,1);
for i = 1:NNODE,
    nx(i) = node(i,2);
    ny(i) = node(i,3);
end
load elem.dat
NELEM = size(elem,1);
for i = 1:NELEM,
    ncon(i,1) = elem(i,2);
    ncon(i,2) = elem(i,3);
    A(i) = elem(i,4);   
    E(i) = elem(i,5);
    lx = nx(ncon(i,2)) - nx(ncon(i,1));
    ly = ny(ncon(i,2)) - ny(ncon(i,1));
    L(i) = sqrt(lx^2+ly^2);
end
load forces.dat
F = zeros(2*NNODE,1);
for i = 1:size(forces,1),
    forceID = 2*(forces(i,2)-1) + forces(i,3);
    F(forceID) = forces(i,4);
end
% load virfor.dat
% T = zeros(2*NNODE,1);
% for i = 1:size(virfor,1),
%     forceID = 2*(virfor(i,2)-1) + virfor(i,3);
%     T(forceID) = virfor(i,4);
% end
load dispbc.dat
for i = 1:size(dispbc,1),
    dispID(i) = 2*(dispbc(i,2)-1) + dispbc(i,3);
end
dispID = dispID';
%
% [u,Rdisp,RF,Ksing,SE,SEgrad] = ...
%     femtruss(A, L, E, nx, ny, ncon, NELEM, NNODE, F, dispID);
%% check determinacy condition for 2D truss structure
a = 2*NNODE-NELEM;
b = 3;
if a==b
    disp('truss structure is statically determinate');
else
    disp('truss structure is statically indeterminate');
    %break;
end

%% Forces in each element with it's nature

[u,Rdisp,RF,Ksing,SE,SEgrad] = ...
    femtruss(A, L, E, nx, ny, ncon, NELEM, NNODE, F, dispID);
P = zeros(NELEM,1);
for i = 1:NELEM,
    P(i) = RF(i,5)*sqrt(RF(i,1)^2+RF(i,2)^2);
end
% %% Due to desired displacement dummy load in each element
% 
% [u,Rdisp,RT,Ksing,SE,SEgrad] = ...
%     femtruss(A, L, E, nx, ny, ncon, NELEM, NNODE, T, dispID);
% t =  zeros(NELEM,1);
% for i = 1:NELEM,
%     t(i) = RT(i,5)*sqrt(RT(i,1)^2+RT(i,2)^2);
% end
%%
% Plotting truss and its deformation
%figure(1)
subplot(2,2,1)
%clf
for i = 1:NELEM,
    eye = ncon(i,1);
    jay = ncon(i,2);
    plot([nx(eye) nx(jay)],[ny(eye) ny(jay)],'-k');
    hold on
    plot([nx(eye) nx(jay)],[ny(eye) ny(jay)],'ko');
    dx1 = u(2*eye-1);
    dx2 = u(2*jay-1);
    dy1 = u(2*eye);
    dy2 = u(2*jay);
    plot([nx(eye)+dx1 nx(jay)+dx2],[ny(eye)+dy1 ny(jay)+dy2],'-r');
    plot([nx(eye)+dx1 nx(jay)+dx2],[ny(eye)+dy1 ny(jay)+dy2],'ro');
end
%title('Subplot 1: Undeformed and Deformed truss structure')
axis equal
axis off
%%  Objective is minimize Strain Energy for given Volume/Weight constraint


%figure(2)% Plotting lagrange multiplier and langrangian value
subplot(2,2,4)
rho = 920;%Polypropylene density
W_star = .18; %Maximum weight 
len_s = L;%L/1000;% length dimensions in mm and node's distance mention in mm
%%
%% Find optimum multiplier value by dual method 
G_opt_dl = 0;
for k=1:length(P)
    G_opt_dl = G_opt_dl + ((len_s(k)/W_star)*sqrt(rho*P(k)*P(k)/(2*E(k))));
end
G_opt = G_opt_dl*G_opt_dl;
G_opt
G_m = linspace(0.001*G_opt,10*G_opt,10000);% Range of this we need to define
 for i=1:length(G_m)
         sum_se(i)=0;
          sum_w(i) = 0;
         for k=1:length(P)
             A(k)=(sqrt((P(k)*P(k))/(2*G_m(i)*rho*E(k))));
         end
         for k=1:length(P)
             if A(k) ~= 0
             sum_se(i) = sum_se(i)+(P(k)*P(k)*len_s(k)/(2*A(k)*E(k)));
             end
         end
         for k=1:length(P)
             sum_w(i) = sum_w(i) + rho*A(k)*len_s(k);
         end
         Lang(i) = sum_se(i)+G_m(i)*(sum_w(i)-W_star);    
 end
 plot(G_m,Lang,'linewidth',2)
 xlabel('\Gamma')
 ylabel('Lagrangian')
   hold on
[M,Ind] = max(Lang);
max_del = G_m(Ind)% lagrange multiplier value for stiff structure
%% optimized area and min strain Energy 
sum_wt =0;
Se_opt = 0;
 for k=1:length(P)
          Ar(k) = sqrt(P(k)*P(k)/(2*G_m(Ind)*rho*E(k)));
          if Ar(k) ~= 0
          sum_wt = sum_wt + rho*Ar(k)*len_s(k);
          Se_opt = Se_opt + P(k)*P(k)*L(k)/(2*E(k)*Ar(k));
          end
 end
 Weight_min = sum_wt % check this for 
 Se_opt % min Strain Energy(N.m) in  for calculated lagrange multiplier
 %weight_int = rho*1E-6*sum(len_s)
 plot(max_del,M,'*','DisplayName','Lagrangian')%plot(max_del,Weight_min,'*')
 %title('Subplot 2: “Lagrangian” vs “Gamma”')
 
 %hold off
 % plot objective versus multiplier
%  hold on
%  plot(G_m,rho*sum_vl,'-r','DisplayName','Weight')
%  legend('Lagrangian','Weight')
%%
%% PLOTING THE OPTIMIZED STRUCTURE
Ar2=[];
% for i=1:length(Ar)
%     if(Ar(i)>=1e-15)
%         Ar2=[Ar2,Ar(i)]
%         
%     end
% end

[NELEM1 NODE] = size(ncon);
tol = 1E-15;
for i = 1:NELEM1
        if(Ar(i)>=tol)
        Ar2=[Ar2,Ar(i)];
        
    end
%     ind1 = find(Ar<=tol);% by sorting 
%     Ar2 = veccut(Ar,ind1);
end

    minA = min(Ar2);
    Ar2_sc = Ar2/minA;
    
for i = 1:NELEM1
    eye = ncon(i,1); jay = ncon(i,2);
%     if(Ar(i)>=tol)
%         Ar2=[Ar2,Ar(i)];
%         
%     end
% %     ind1 = find(Ar<=tol);% by sorting 
% %     Ar2 = veccut(Ar,ind1);
%     minA = min(Ar2);
    
    if Ar(i)<=tol
        lw = 0.001;
        lc = '--b';
    else
        lw = 1+4*(Ar(i)-min(Ar))/(max(Ar)-min(Ar));
        lc = '-k';
    end
    %lw = 1E4*abs(Ar(i));
    %figure(3)
    subplot(2,2,2)
    hold on;
    plot([nx(eye) nx(jay)], [ny(eye) ny(jay)],lc,'linewidth',lw);
    hold on
end
%title('Subplot 3: Optimized truss structure')
axis equal
axis off

% 
% 
% Ae = Ar;
% j = 1;
% for i = 1:NELEM
%     
%     if Ae(i) >= 1e-15
%              NCON(j,:) = ncon(i,:);
%              AE(j) = Ae(i);
%              j = j+1;
%     end
%     
% end
% 
% 
% 
% [NELEM1 NODE] = size(NCON);
% for i = 1:NELEM1
%     eye = NCON(i,1); jay = NCON(i,2);
%     lw = AE(i);
%     figure(3)
%     hold on;
%     plot([nx(eye) nx(jay)], [ny(eye) ny(jay)],'-r','linewidth',lw);
%     hold on
% end

