function [] = hw5_sub()

global   nn ne nm ndim nep nqn
global   nq nf nmpc
global   XYZ CTAB AE QTAB FTAB MATERIAL
global   FIDI_NAME ns
global   k l m n K L F P Q


%10 Read Input Data File for 2D truss and calculate ns, the order of the system (total number of displacements).
ANSWER = input('Input Data File Name: ', 's');	%Get typed name of FIDI
if size(ANSWER) ~= 0
    FIDI_NAME = ANSWER
end
Get_3D_Truss_Data(FIDI_NAME);

%2  Write a script to calculate bar lengths Lx, Ly, L, l=cos(theta) and m=sin(theta) for each of the ne elements.
%Store the results in (1 x ne) row vectors [L.e], [l.e] and [m.e].
Bar_Lengths;

%3  Write a script to draw the truss by plotting each element (graph the endpoints) in the same figure.
figure(1);
hold on; axis equal;
Draw_Truss
Label_Nodes
hold off

%4  Write a script to calculate k=AE/L for each element.
%Store the results in (l x ne) row vector [k.e]
for e=1:ne
    k(e)=AE(e)/L(e);
end
k

%5  Write a script, given nf and FTAB, to initialize the column vector F with the specified applied forces.
%Note: an applied force is zero unless some other value is specified.
F=zeros(ns,1);
for i=1:nf
    F(FTAB(i,1))=FTAB(i,2);
end
F

%6  Write a Script, given nq and QTAB, to initialize the column vector Q with the specified displacements.
%Note: To create Q a value must be given. For now, let the Q(i) be zero except where another value is given.
Q=zeros(ns,1);
for i=1:nq
    Q(QTAB(i,1))=QTAB(i,2);
end
Q

%7  Calculate Ke and Ge for each element. Assemble the results in K=Sum(Ge'*Ke*Ge)
K=zeros(ns,ns);
Ge=zeros(2*nqn,ns);
for e=1:ne
    M=[l(e) m(e) n(e) 0 0 0; 0 0 0 l(e) m(e) n(e)];
    Ke=k(e)*M'*[1 -1; -1 1]*M
    Ge=0*Ge;
    Ge(1,3*CTAB(e,1)-2)=1;
    Ge(2,3*CTAB(e,1)-1)=1;
    Ge(3,3*CTAB(e,1)  )=1;
    Ge(4,3*CTAB(e,2)-2)=1;
    Ge(5,3*CTAB(e,2)-1)=1;
    Ge(6,3*CTAB(e,2)  )=1
    K=K+Ge'*Ke*Ge
end

%8 Apply the Boundary Conditions and solve for displacements QF and reactions F
% Include QF in Q and FE in F
QF_Solver

% Calculate the nodal forces for each element Fe=Ke*Qe 
% Find the bar forces P(e) for each element
P=zeros(ne,1)
for e=1:ne
    M=[l(e) m(e) n(e) 0 0 0; 0 0 0 l(e) m(e) n(e)];
    Ke=k(e)*M'*[1 -1; -1 1]*M
    Ge=0*Ge;
    Ge(1,3*CTAB(e,1)-2)=1;
    Ge(2,3*CTAB(e,1)-1)=1;
    Ge(3,3*CTAB(e,1)  )=1;
    Ge(4,3*CTAB(e,2)-2)=1;
    Ge(5,3*CTAB(e,2)-1)=1;
    Ge(6,3*CTAB(e,2)  )=1
    Fe=Ke*Ge*Q
    P(e)=[l(e) m(e) n(e)]*Fe(4:6)
end

%Draw truss in solid lines, label nodes, draw deformed truss in broken
%lines with green tension members and red compression members
figure(2);
hold on; axis equal;
Draw_Truss
Label_Nodes
Draw_Deformed_Truss
hold off

%=====
function []= Get_3D_Truss_Data(FIDI_NAME)
%GetData2D(FIDI_NAME) reads input data for 2D truss from FIDI_NAME.
%FIDI_NAME (FIDI ~ File ID for Input) is the name of the input data file.
global   nn ne nm ndim nep nqn
global   nq nf nmpc
global   XYZ CTAB AE QTAB FTAB MATERIAL
global   ns


FIDI = fopen(FIDI_NAME, 'r');	%Open input file
disp(blanks(1));		%Write blanks to screen
TEXT = fgets(FIDI)		%Read captions, 4 lines
TEXT = fgets(FIDI)
TEXT = fgets(FIDI)
TEXT = fgets(FIDI)
DATA = str2num(fgets(FIDI));
[nn ne nm ndim nep nqn] = deal(DATA(1), DATA(2), DATA(3), DATA(4), DATA(5), DATA(6))
TEXT = fgets(FIDI)
DATA = str2num(fgets(FIDI));
[nq nf nmpc] = deal(DATA(1), DATA(2), DATA(3));

TEXT = fgets(FIDI)
XYZ = zeros(nn,3);		%make node coordinates table
for i=1:nn
    DATA = str2num(fgets(FIDI));
    for j=1:ndim
        XYZ(DATA(1),j) = DATA(1+j);
    end
end

TEXT = fgets(FIDI)
CTAB = zeros(ne,2);		%make CTAB: connectivity table
AE = zeros(ne,1); 		%make AE: members property table
for i=1:ne
    DATA = str2num(fgets(FIDI));
    [CTAB(DATA(1),1) CTAB(DATA(1),2) AE(DATA(1))] = deal(DATA(2), DATA(3), DATA(4));
end

TEXT = fgets(FIDI)
QTAB = zeros(nq,2);		%make QTAB: essential displacements table
for i=1:nq
    DATA = str2num(fgets(FIDI));
    [QTAB(i,1) QTAB(i,2)] = deal(DATA(1), DATA(2));
end

TEXT = fgets(FIDI)
FTAB = zeros(nf,2);		%make FTAB: specified forces table
for i=1:nf
    DATA = str2num(fgets(FIDI));
    [FTAB(i,1) FTAB(i,2)] = deal(DATA(1), DATA(2));
end

TEXT = fgets(FIDI)
MATERIAL = zeros(nm,2);		%make FTAB: specified forces table
for i=1:nm
    DATA = str2num(fgets(FIDI));
    nmp=1;				%number of material properties
    for j=1:nmp
        MATERIAL(DATA(1),j) = DATA(1+j);
    end
end

TEXT = fgets(FIDI)
status = fclose('all');
ns = nn*nqn
return;
%==========


function [] = Bar_Lengths()
%Function calculates bar lengths Lx, Ly, L, l=cos(theta) and m=sin(theta) for each of the ne elements.
%Store the results in (1 x ne) row vectors [L.e], [l.e] and [m.e].
global ne l m n L CTAB XYZ

for e=1:ne
    Lx=XYZ(CTAB(e,2),1)-XYZ(CTAB(e,1),1)
    Ly=XYZ(CTAB(e,2),2)-XYZ(CTAB(e,1),2)
    Lz=XYZ(CTAB(e,2),3)-XYZ(CTAB(e,1),3)
    L(e)=sqrt(Lx*Lx+Ly*Ly+Lz*Lz)
    l(e)=Lx/L(e)
    m(e)=Ly/L(e)
    n(e)=Lz/L(e)
end
return;
%==========

function [] = Draw_Truss()
%draw 2D truss

global XYZ CTAB ne 

Xmin=XYZ(CTAB(1,1),1);
Ymin=XYZ(CTAB(1,1),2);
Zmin=XYZ(CTAB(1,1),3);
Xmax=Xmin;
Ymax=Ymin;
Zmax=Zmin;
for e=1:ne
    Xe2=XYZ(CTAB(e,2),1);
    Xe1=XYZ(CTAB(e,1),1);
    Ye2=XYZ(CTAB(e,2),2);
    Ye1=XYZ(CTAB(e,1),2);
    Ze2=XYZ(CTAB(e,2),3);
    Ze1=XYZ(CTAB(e,1),3);    
    plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2]);
    Xmin=min([Xmin Xe1 Xe2]);
    Xmax=max([Xmax Xe1 Xe2]);
    Ymin=min([Ymin Ye1 Ye2]);
    Ymax=max([Ymax Ye1 Ye2]); 
    Zmin=min([Zmin Ze1 Ze2]);
    Zmax=max([Zmax Ze1 Ze2]); 
end
e=max(Xmax-Xmin,Ymax-Ymin);	%add border space
e=max(e,Zmax-Zmin)/32;
plot3(Xmin-e,Ymin-e,Zmin-e);
plot3(Xmax+e,Ymax+e,Zmax+e);
return;
%==========

function [] = Label_Nodes()
%Label nodes
global XYZ nn
for i=1:nn
    text(XYZ(i,1), XYZ(i,2), XYZ(i,3), num2str(i));
end
return;
%==========

function [] = Draw_Deformed_Truss()
%draw deformed truss
global XYZ CTAB P Q ne

mag=200
for e=1:ne
    n1=CTAB(e,1);
    n2=CTAB(e,2);
    Xe2=XYZ(n2,1)+mag*Q(3*n2-2);
    Xe1=XYZ(n1,1)+mag*Q(3*n1-2);
    Ye2=XYZ(n2,2)+mag*Q(3*n2-1);
    Ye1=XYZ(n1,2)+mag*Q(3*n1-1);
    Ze2=XYZ(n2,3)+mag*Q(3*n2  );
    Ze1=XYZ(n1,3)+mag*Q(3*n1  );
    if P(e)<0
        plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2], 'r--');  %red compression bars
    else
        plot3([Xe1 Xe2], [Ye1 Ye2], [Ze1 Ze2], 'g--');  %green tension bars
    end
end
return;
%==========

function [] = QF_Solver()
%Given QTAB nq, ns, and the system F=K*Q, transforms to similar system H=C*D
%The essential boundary conditions are in the upper part of D via D=T*Q
%Then C=T*K*T' and H=T*F. H=C*D is solved for D by partitions
%Reverse transformation gives Q=T'*D and F=T'*H
global QTAB F K Q nq ns
 
%In truth table I, I(i) is 1 if Q(i) is an essential displacement in QTAB.
I=zeros(ns,1);
for i=1:nq
    I(QTAB(i,1))=1;
end
I
%Copy the indices of the essential displacenemts from QTAB to V(1:nq)
V=zeros(ns,1);
V(1:nq,1) = QTAB(:,1);
%Write the remaining indices in V(nq+1:ns)
j=nq+1;
for i=1:ns
    if I(i)==0
        V(j)=i;
        j=j+1;
    end
end
V
%Make transform T from the indices in V. 
T=zeros(ns)
for i=1:ns
    T(i,V(i))=1;
end
T
%find the similar system H=CD using the transform matrix T
D=T*Q
H=T*F
C=T*K*T';
%Solve by partitions. The free displacements are DF=inv(CF)*(HF-CEF'*DE)
DE=D(1:nq);
CE=C(1:nq,1:nq);
CF=C(nq+1:ns,nq+1:ns);
CEF=C(1:nq,nq+1:ns);
HF=H(nq+1:ns);
DF=inv(CF)*(HF-CEF'*DE);
%The Reactions are HE=CE*DE+CEF*DF. Insert parts DF in D and HE in H
HE=CE*DE+CEF*DF;
D(nq+1:ns)=DF
H(1:nq)=HE
%Transform the results back to the original
Q=T'*D
F=T'*H
return;
%==========