clear
clc

%Solution of exercise 1a Week#5

%% Properties
E=200000;
l=4000;
I=3.66*10^7;
A=12700;

%% Equation number
nb_el=1;
elements=[1:nb_el];
connectivity=[1 2];

numEq = zeros(size(connectivity, 1), 3*size(connectivity, 2));
for e = 1:nb_el
    numEq(e, :) = (3 * (e - 1) + 1):(3 * (e - 1) + 6);
end

%% Transformation matrix from local to global
C = cos(pi/2);
S = sin(pi/2);

T = [ C,  S, 0, 0, 0, 0;
    -S,  C, 0, 0, 0, 0;
    0,  0, 1, 0, 0, 0;
    0,  0, 0, C,  S, 0;
    0,  0, 0, -S, C, 0;
    0,  0, 0, 0,  0, 1];

%% Elastic stiffness matrix in basic reference frame
Kbar = [E*A/l 0 0;
    0  4*E*I/l 2*E*I/l;
    0 2*E*I/l 4*E*I/l];

%% Elastic stiffness matrix in local reference frame
Ke_loc = [ E*A/l,         0,          0,        -E*A/l,         0,           0;
    0,   12*E*I/l^3,   6*E*I/l^2,         0, -12*E*I/l^3,   6*E*I/l^2;
    0,    6*E*I/l^2,    4*E*I/l,         0,  -6*E*I/l^2,    2*E*I/l;
    -E*A/l,         0,          0,         E*A/l,         0,           0;
    0,  -12*E*I/l^3,  -6*E*I/l^2,         0,  12*E*I/l^3,  -6*E*I/l^2;
    0,    6*E*I/l^2,    2*E*I/l,         0,  -6*E*I/l^2,    4*E*I/l];

%% Initial total structure stiffness matrix
Ke_glob=T'*Ke_loc*T;

Kg_glob=zeros(3*(size(elements, 2)+1));

Ktot=Ke_glob+Kg_glob;

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

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

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

%% Definition of the boundary conditions
alpha=0.05;
F_ext=zeros(3*(size(elements, 2)+1),1);
F_ext(4)=alpha;
F_ext(5)=-1;
allDofs=[1:3*(size(elements, 2)+1)];
fixedDofs=[1 2 3];
freeDOfs=setdiff(allDofs,fixedDofs);

%% Force control
lambda_max=1100000;
nIncrements=100;
DeltaLambdaBar=lambda_max/nIncrements;
tol=1e-4;
nIterMax=1000;

%% Solve the system
n=1;
while n<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=DeltaLambdaBar;
        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=0;
        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
        u=T*v;
        
%         [uBar]=computeTransfoLocalToBasic_linear(l,u);
        [uBar]=computeTransfoLocalToBasic_corot(l,u);
          
        qBar=Kbar*uBar;
        
        [L,Kg]=computeTransfoBasicToLocal_corot(l,u,qBar);
% [L,Kg]=computeTransfoBasicToLocal_linear(l);
        
        Q=L'*qBar;
        F_int=T'*Q;
        
        Ktot=T'*(L'*Kbar*L+Kg)*T;
        
        
        % 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(:,4),lambda_Vector/1000,'--k','LineWidth',1.2)
xlabel('u_b [mm]')
ylabel('P [kN]')
grid on

% plot_settings_ASCE(h1)


%% Compare linear vs corotational 
% resLinear=importdata('results_week5Exo1_pPos_linear.txt');
% resCorot=importdata('results_week5Exo1_pPos_corot.txt');
% 
% h2=figure;
% plot(resLinear(:,1),resLinear(:,2)/1000,'-k')
% hold on
% plot(resCorot(:,1),resCorot(:,2)/1000,'--r','LineWidth',1.2)
% legend('Linear','Corotational','Location','best')
% xlabel('u_b [mm]')
% ylabel('P [kN]')
% grid on
% 
% plot_settings_ASCE(h2)

%% Export results
% writematrix([v_Vector(:,4),lambda_Vector'], 'results_week5Exo1_pPos_linear.txt');


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

uBar=L*u;

end


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

Kg=zeros(6);

end


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

u1Bar=Ln-l;
u2Bar=u(3)-beta;
u3Bar=u(6)-beta;
uBar=[u1Bar,u2Bar,u3Bar]';

end


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

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

L = [
    -c, -s, 0, c, s, 0;
    -s/Ln, c/Ln, 1, s/Ln, -c/Ln, 0;
    -s/Ln, c/Ln, 0, s/Ln, -c/Ln, 1
];

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

term2 = ((qBar(2) + qBar(3)) / Ln^2) * [
    -2*s*c, c^2 - s^2, 0, 2*s*c, -c^2 + s^2, 0;
    c^2 - s^2, 2*c*s, 0, -c^2 + s^2, -2*c*s, 0;
    0, 0, 0, 0, 0, 0;
    2*s*c, -c^2 + s^2, 0, -2*s*c, c^2 - s^2, 0;
    -c^2 + s^2, -2*c*s, 0, c^2 - s^2, 2*c*s, 0;
    0, 0, 0, 0, 0, 0
];
Kg=term1+term2;

end