clear
clc

%Solution of question 3a Week#6

%% Properties
E=200000;
h=3000;
H=4000;
%Member ab
l_ab=5000;
b_ab=500;
A_ab=b_ab^2;
%Member bc
l_bc=l_ab;
b_bc=1000;
A_bc=b_bc^2;
%Member bd
l_bd=l_ab;
A_bd=A_ab;

%% Equation number
nDof=2;

nb_el=3;
elements=[1:1:nb_el];
connectivity=[1 2; 2 3; 2 4];
numEq = [1 2 3 4;
    3 4 5 6;
    3 4 7 8];

%% Transformation matrix from local to global
%Member ab
c_ab = H/l_ab;
s_ab = h/l_ab;
T_ab = [ c_ab,  s_ab, 0, 0;
         -s_ab,  c_ab, 0, 0;
          0,  0, c_ab, s_ab;
          0,  0, -s_ab, c_ab];
%Member bc
c_bc = cos(pi/2);
s_bc = sin(pi/2);
T_bc = [ c_bc,  s_bc, 0, 0;
         -s_bc,  c_bc, 0, 0;
          0,  0, c_bc, s_bc;
          0,  0, -s_bc, c_bc];
%Member bd
c_bd = H/l_bd;
s_bd = -h/l_bd;
T_bd = [ c_bd,  s_bd, 0, 0;
         -s_bd,  c_bd, 0, 0;
          0,  0, c_bd, s_bd;
          0,  0, -s_bd, c_bd];

%% Elastic stiffness matrix in basic reference frame
Kbar_ab= [E*A_ab/l_ab];
Kbar_bc= [E*A_bc/l_bc];
Kbar_bd= [E*A_bd/l_bd];

%% Elastic stiffness matrix in local reference frame
Ke_loc_ab = (E * A_ab / l_ab) * [1, 0, -1, 0;
                       0, 0,  0, 0;
                      -1, 0,  1, 0;
                       0, 0,  0, 0];
Ke_loc_bc = (E * A_bc / l_bc) * [1, 0, -1, 0;
                       0, 0,  0, 0;
                      -1, 0,  1, 0;
                       0, 0,  0, 0];
Ke_loc_bd = (E * A_bd / l_bd) * [1, 0, -1, 0;
                       0, 0,  0, 0;
                      -1, 0,  1, 0;
                       0, 0,  0, 0];
                   
% % Test
% [L,~]=computeTransfoBasicToLocal_corot(l_ab,zeros(6,1),zeros(3,1));
% Ktest=L'*Kbar_ab*L;
% test=norm(Ktest-Ke_loc_ab);

%% Initial total structure stiffness matrix
Kg_glob_elem=zeros(2*nDof);

% Assemble the global stiffness matrix using the equation number matrix
Ktot=zeros(nDof*(size(elements, 2)+1));
%Member ab
dof_ab=numEq(1,:);
Ktot(dof_ab, dof_ab) = Ktot(dof_ab, dof_ab) + T_ab'*(Ke_loc_ab+Kg_glob_elem)*T_ab;
%Member bc
dof_bc=numEq(2,:);
Ktot(dof_bc, dof_bc) = Ktot(dof_bc, dof_bc) + T_bc'*(Ke_loc_bc+Kg_glob_elem)*T_bc;
%Member bd
dof_bd=numEq(3,:);
Ktot(dof_bd, dof_bd) = Ktot(dof_bd, dof_bd) + T_bd'*(Ke_loc_bd+Kg_glob_elem)*T_bd;

%% Initialize variables
v_Vector=[];
F_int_Vector=[];
lambda_Vector=[];

F_int=zeros(nDof*(size(elements, 2)+1),1);
lambda=0;
v=zeros(nDof*(size(elements, 2)+1),1);
F_unb=zeros(nDof*(size(elements, 2)+1),1);
F_ext_Tot=zeros(nDof*(size(elements, 2)+1),1);

DeltaV_u=zeros(nDof*(size(elements, 2)+1),1);
DeltaV_f=zeros(nDof*(size(elements, 2)+1),1);

%% Definition of the boundary conditions
F_ext=zeros(nDof*(size(elements, 2)+1),1);
F_ext(6)=-1;
allDofs=[1:nDof*(size(elements, 2)+1)];
fixedDofs=[1 2 5 7 8];
freeDOfs=setdiff(allDofs,fixedDofs);

%% displacement control
vMax=-8000;
nIncrements=500;
DeltaVBar=vMax/nIncrements;
q_ctrDof=6; % Control DOF
nIterMax=100;
tol=1e-0;

%% Solve the system
for n=1:nIncrements
    converged=0;
    i=1;
    
    % Iterations
    while converged==0
        
        if i==1
            % Determine DeltaV_a and DeltaV_b
            DeltaV_u(freeDOfs)=Ktot(freeDOfs,freeDOfs)\-F_unb(freeDOfs);
            DeltaV_f(freeDOfs)=Ktot(freeDOfs,freeDOfs)\F_ext(freeDOfs);
            % Determine in load multiplier DeltaLambda
            DeltaLambda=DeltaVBar/DeltaV_f(q_ctrDof);
        else
            % Determine DeltaV_a and DeltaV_b
            DeltaV_u(freeDOfs)=Ktot(freeDOfs,freeDOfs)\-F_unb(freeDOfs);
            DeltaV_f(freeDOfs)=Ktot(freeDOfs,freeDOfs)\F_ext(freeDOfs);
            % Determine in load multiplier DeltaLambda
            DeltaLambda=-DeltaV_u(q_ctrDof)/DeltaV_f(q_ctrDof);
        end
        
        %Update of load and displacements
        lambda=lambda+DeltaLambda;
        DeltaV=DeltaV_u+DeltaLambda*DeltaV_f;
        v=v+DeltaV;
        
        % Compute stiffness matrix and resisting force vector
        F_int=zeros(nDof*(size(elements, 2)+1),1);
        Ktot=zeros(nDof*(size(elements, 2)+1));
        %Member ab
        u_ab=T_ab*v(dof_ab);
        [uBar_ab]=computeTransfoLocalToBasic_corot(l_ab,u_ab);
        % [uBar_ab]=computeTransfoLocalToBasic_linear(l_ab,u_ab);
        qBar_ab=Kbar_ab*uBar_ab;
        [L_ab,Kg_ab]=computeTransfoBasicToLocal_corot(l_ab,u_ab,qBar_ab);
        % [L_ab,Kg_ab]=computeTransfoBasicToLocal_linear(l_ab);
        % Assemble global stiffness matrix
        Ktot(dof_ab,dof_ab)=Ktot(dof_ab,dof_ab)+T_ab'*(L_ab'*Kbar_ab*L_ab+Kg_ab)*T_ab;
        % Assemble resisting load vector for verification
        F_int(dof_ab)=F_int(dof_ab)+T_ab'*(L_ab'*qBar_ab);
        %Member bc
        u_bc=T_bc*v(dof_bc);
        [uBar_bc]=computeTransfoLocalToBasic_corot(l_bc,u_bc);
        % [uBar_bc]=computeTransfoLocalToBasic_linear(l_bc,u_bc);
        qBar_bc=Kbar_bc*uBar_bc;
        [L_bc,Kg_bc]=computeTransfoBasicToLocal_corot(l_bc,u_bc,qBar_bc);
        % [L_bc,Kg_bc]=computeTransfoBasicToLocal_linear(l_bc);
        % Assemble global stiffness matrix
        Ktot(dof_bc,dof_bc)=Ktot(dof_bc,dof_bc)+T_bc'*(L_bc'*Kbar_bc*L_bc+Kg_bc)*T_bc;
        % Assemble resisting load vector for verification
        F_int(dof_bc)=F_int(dof_bc)+T_bc'*(L_bc'*qBar_bc);
        %Member bd
        u_bd=T_bd*v(dof_bd);
        [uBar_bd]=computeTransfoLocalToBasic_corot(l_bd,u_bd);
        % [uBar_bd]=computeTransfoLocalToBasic_linear(l_bd,u_bd);
        qBar_bd=Kbar_bd*uBar_bd;
        [L_bd,Kg_bd]=computeTransfoBasicToLocal_corot(l_bd,u_bd,qBar_bd);
        % [L_bd,Kg_bd]=computeTransfoBasicToLocal_linear(l_bd);
        % Assemble global stiffness matrix
        Ktot(dof_bd,dof_bd)=Ktot(dof_bd,dof_bd)+T_bd'*(L_bd'*Kbar_bd*L_bd+Kg_bd)*T_bd;
        % Assemble resisting load vector for verification
        F_int(dof_bd)=F_int(dof_bd)+T_bd'*(L_bd'*qBar_bd);
        
        
        % Compute unbalanced force vector
        F_ext_Tot=F_ext_Tot+DeltaLambda*F_ext;
        F_unb=F_int-F_ext_Tot;
        
        % Check for convergence
        test=norm(F_unb(freeDOfs));
        if test<tol %we have converged
            converged=1;
            v_Vector(n,:)=v;
            F_int_Vector(n,:)=F_int;
            lambda_Vector(n)=lambda;
        end
        
        if i==nIterMax && converged==0
            error('Failled to converge')
        end
        i=i+1;
    end
    
    n=n+1;
end



%% Plot the results
h1=figure;
plot(-v_Vector(:,6)/1000,lambda_Vector/1000,'-k','LineWidth',1.2)
xlabel('v_c [m]')
ylabel('P [kN]')
grid on

% plot_settings_ASCE(h1)




%% Compute linear transformation
function [uBar]=computeTransfoLocalToBasic_linear(l,u)
L = [
    -1, 0, 1, 0;
    ];

uBar=L*u;

end


function [l,Kg]=computeTransfoBasicToLocal_linear(l)
l = [
    -1, 0, 1, 0;
    ];

Kg=zeros(2*nDof);

end


%% Compute corotational transformation
function [uBar]=computeTransfoLocalToBasic_corot(l,u)
DeltaUx=u(3)-u(1);
DeltaUy=u(4)-u(2);
Ln=sqrt((l+DeltaUx)^2+(DeltaUy)^2);

u1Bar=Ln-l;

uBar=[u1Bar];

end


function [L,Kg]=computeTransfoBasicToLocal_corot(l,u,qBar)
DeltaUx=u(3)-u(1);
DeltaUy=u(4)-u(2);
Ln=sqrt((l+DeltaUx)^2+(DeltaUy)^2);
beta=atan(DeltaUy/(l+DeltaUx));

c=cos(beta);
s=sin(beta);

L = [
    -c, -s, c, s;
    ];

term1 = (qBar(1) / Ln) * [
    s^2, -c*s, -s^2, c*s;
    -c*s, c^2, c*s, -c^2;
    -s^2, c*s, s^2, -c*s;
    c*s, -c^2, -c*s, c^2;
    ];

Kg=term1;

end