clear
clc

%Solution of exercise 1a Week#5

%% Properties
%Member
E=200000;
l=4000;
I=3.66*10^7;
A=12700;
ke_mem=3*E*I/l;
thetaP_mem=0.02;
thetaPc_mem=0.05;
My=4000*1000;
Mu=4500*1000;
% My=200000*1000;
% Mu=225000*1000;
thetaY_mem=My/ke_mem;
thetaC_mem=thetaP_mem+thetaY_mem;
thetaU_mem=thetaPc_mem+thetaC_mem;
as_mem=(Mu-My)/(thetaP_mem*ke_mem);
apc_mem=-Mu/(thetaPc_mem*ke_mem);

% Spring and elastic beam-column element
nSpring=10;
Ie=(nSpring+1)/nSpring*I;
ke_s=nSpring*3*E*Ie/l;
ks_s=as_mem/(1+nSpring*(1-as_mem))*ke_s;
kpc_s=apc_mem/(1+nSpring*(1-apc_mem))*ke_s;
thetaY_s=My/ke_s;
thetaP_s  = (Mu-My)/ks_s;% Pre-capping Plastic rotation of the spring
thetaPc_s = (0-Mu)/(kpc_s);% Post-capping plastic rotation of the spring
thetaC_s=thetaP_s+thetaY_s;
thetaU_s=thetaPc_s+thetaC_s;

%% Equation number
dofs_spring=[1 4];
dofs_bc=[2:7];

%% 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
S22=12*nSpring/(3*nSpring-1);
S23=6*nSpring/(3*nSpring-1);
S32=6*nSpring/(3*nSpring-1);
S33=12*nSpring^2/(3*nSpring^2+2*nSpring-1);
Kbar = [E*A/l 0 0;
    0  S22*E*Ie/l S23*E*Ie/l;
    0 S32*E*Ie/l S33*E*Ie/l];

%% Elastic stiffness matrix in local reference frame
[L,Kg]=computeTransfoBasicToLocal_linear(l);
Ke_loc = L'*Kbar*L;

KeSpring=ke_s*[1 -1; -1 1];

%% Initial total structure stiffness matrix
Ktot=zeros(7);
Ktot(dofs_bc,dofs_bc)=Ktot(dofs_bc,dofs_bc)+T'*Ke_loc*T;
Ktot(dofs_spring,dofs_spring)=Ktot(dofs_spring,dofs_spring)+KeSpring;

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

F_int=zeros(7,1);
lambda=0;
v=zeros(7,1);
F_unb=zeros(7,1);
F_ext_Tot=zeros(7,1);

DeltaV_u=zeros(7,1);
DeltaV_f=zeros(7,1);

theta_previous=0;
M_previous=0;
MmaxPos=My;
MmaxNeg=-My;
yieldFlag_Pos=0;
cappingFlag_Pos=0;
yieldFlag_Neg=0;
cappingFlag_Neg=0;
reversalFlag=0;
residualFlag=0;
theta_M0_currentPos=0;
theta_M0_currentNeg=0;
theta_M0_projected=0;
Di_previous=0;

%% Definition of the boundary conditions
alpha=0.05;
F_ext=zeros(7,1);
F_ext(5)=alpha;
F_ext(6)=-1;
allDofs=[1:7];
fixedDofs=[1 2 3];
freeDOfs=setdiff(allDofs,fixedDofs);

%% displacement control
uTopMax=0.12*l;
nIncrements=1000;
DeltaVBar=uTopMax/nIncrements;
q_ctrDof=5; % Control DOF
tol=1e-4;
nIterMax=1000;

MSpring_vector=zeros(nIncrements,1);

%% Solve the system
n=1;
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(7,1);
        Ktot=zeros(7);
        %Elastic beam-column element
        u=T*v(dofs_bc);
        [uBar]=computeTransfoLocalToBasic_linear(l,u);
%                 [uBar]=computeTransfoLocalToBasic_corot(l,u);
        qBar=Kbar*uBar;
        [L,Kg]=computeTransfoBasicToLocal_linear(l);
%                 [L,Kg]=computeTransfoBasicToLocal_corot(l,u,qBar);
        Q=L'*qBar;
        F_int(dofs_bc)=F_int(dofs_bc)+T'*Q;
        Ktot(dofs_bc,dofs_bc)=Ktot(dofs_bc,dofs_bc)+T'*(L'*Kbar*L+Kg)*T;
        % Inelastic spring
        theta=v(dofs_spring(2))-v(dofs_spring(1));
        [fSpring,kSpring,MmaxPos,MmaxNeg,yieldFlag_Pos,cappingFlag_Pos,yieldFlag_Neg,cappingFlag_Neg,reversalFlag,residualFlag,theta_M0_currentPos,theta_M0_currentNeg,theta_M0_projected,Di]...
            =computeTrilinearMatLaw(theta,theta_previous,M_previous,theta_M0_currentPos,theta_M0_currentNeg,theta_M0_projected,ke_s,ks_s,kpc_s,thetaY_s,thetaC_s,thetaU_s,My,Mu,MmaxPos,MmaxNeg,yieldFlag_Pos,cappingFlag_Pos,yieldFlag_Neg,cappingFlag_Neg,reversalFlag,residualFlag,Di_previous);
%         test1=KeSpring*v(dofs_spring);
%         test2=[-fSpring;fSpring];
        F_int(dofs_spring)=F_int(dofs_spring)+[-fSpring;fSpring];
        Ktot(dofs_spring,dofs_spring)=Ktot(dofs_spring,dofs_spring)+kSpring*[1 -1; -1 1];
        
        
        % 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;
            
            % Update the spring variables
            MSpring_vector(n)=fSpring;
            M_previous=fSpring;
            theta_previous=theta;
            Di_previous=Di;
        end
        
        if i==nIterMax && converged==0
            error('Failled to converge')
        end
        i=i+1;
    end
    
end



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

% plot_settings_ASCE(h1)


%% Compare linear vs corotational 
% res_LG_LM=importdata('results_week5Exo1_pNeg_LG_LM.txt');
% res_NlG_LM=importdata('results_week5Exo1_pNeg_NlG_LM.txt');
% % res_LG_NlM=importdata('results_week7Exo3_pNeg_LG_NlM.txt');
% % res_NlG_NlM=importdata('results_week7Exo3_pNeg_NlG_NlM.txt');
% res_LG_NlM=importdata('results_week7Exo3_pNeg_LG_NlM_50My.txt');
% res_NlG_NlM=importdata('results_week7Exo3_pNeg_NlG_NlM_50My.txt');
% 
% h2=figure;
% plot([0;res_LG_LM(:,1)]/l,[0;res_LG_LM(:,2)]/1000,'-')
% hold on
% plot([0;res_NlG_LM(:,1)]/l,[0;res_NlG_LM(:,2)]/1000,'--','LineWidth',1.1)
% plot([0;res_LG_NlM(:,1)]/l,[0;res_LG_NlM(:,2)]/1000,'-.','LineWidth',1.2)
% plot([0;res_NlG_NlM(:,1)]/l,[0;res_NlG_NlM(:,2)]/1000,':','LineWidth',1.3)
% % legend('LG LM','NlG LM','LG NlM','NlG NlM','Location','best')
% legend('Case#1','Case#2','Case#3','Case#4','Location','best')
% xlabel('u_b/h [-]')
% ylabel('P [kN]')
% grid on
% % xlim([0,0.12])
% % ylim([0,30]);
% xlim([0,0.15])
% ylim([0,1200]);
% 
% plot_settings_ASCE(h2)

%% Export results
% writematrix([v_Vector(:,5),lambda_Vector'], 'results_week7Exo3_pNeg_LG_NlM_50My.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


%% Function to return the moment and tangent of the inelastic spring
function [M,k,MmaxPos,MmaxNeg,yieldFlag_Pos,cappingFlag_Pos,yieldFlag_Neg,cappingFlag_Neg,reversalFlag,residualFlag,theta_M0_currentPos,theta_M0_currentNeg,theta_M0_projected,Di]...
    =computeTrilinearMatLaw(theta,theta_previous,M_previous,theta_M0_currentPos,theta_M0_currentNeg,theta_M0_projected,ke,ks,kpc,thetaY,thetaC,thetaU,My,Mu,MmaxPos,MmaxNeg,yieldFlag_Pos,cappingFlag_Pos,yieldFlag_Neg,cappingFlag_Neg,reversalFlag,residualFlag,Di_previous)
    % Find the direction of the current increment "Di": 1:if Ri is moving towards positive and -1: if Ri is moving negative
    if theta>=theta_previous
        Di=1;
	else
		Di=-1;
    end
    
    % Trial elastic moment
    M=M_previous+ke*(theta-theta_previous);
    
    % Quick return if failed
    if residualFlag==1
        M=0;
        k=10;
        return
    end
    
    %If inelastic and going to positive rotation
    if Di==1 && M>MmaxPos
        yieldFlag_Neg=0;
        cappingFlag_Neg=0;
        if yieldFlag_Pos==1 || cappingFlag_Pos==1
        else
            theta_M0_currentPos=theta_M0_projected;
        end
        
        if abs(theta_M0_currentPos-theta)>=thetaY && abs(theta_M0_currentPos-theta)<thetaC %hardening path
            yieldFlag_Pos=1;
            theta_M0_currentNeg=theta_M0_projected;
            M=Di*My+ks*(theta-theta_M0_currentPos-thetaY);
            MmaxNeg=-My;
        elseif abs(theta_M0_currentPos-theta)>=thetaC && abs(theta_M0_currentPos-theta)<thetaU %softening path
            yieldFlag_Pos=0;
            cappingFlag_Pos=1;
            M=Di*Mu+kpc*(theta-theta_M0_currentPos-thetaC);
        else %residual path
            residualFlag=1;
            M=0;
        end
    
        MmaxPos=M;
    end
    
    
    %If inelastic and going to negative rotation
    if Di==-1 && M<MmaxNeg
        yieldFlag_Pos=0;
        cappingFlag_Pos=0;
        if yieldFlag_Neg==1 || cappingFlag_Neg==1
        else
            theta_M0_currentNeg=theta_M0_projected;
        end
        
        if abs(theta_M0_currentNeg-theta)>=thetaY && abs(theta_M0_currentNeg-theta)<thetaC %hardening path
            yieldFlag_Neg=1;
            theta_M0_currentPos=theta_M0_projected;
            M=Di*My+ks*(theta-theta_M0_currentNeg+thetaY);
            MmaxPos=My;
        elseif abs(theta_M0_currentNeg-theta)>=thetaC && abs(theta_M0_currentNeg-theta)<thetaU
            yieldFlag_Neg=0;
            cappingFlag_Neg=1;
            M=Di*Mu+kpc*(theta-theta_M0_currentNeg+thetaC);
        else %failed
            residualFlag=1;
            M=0;
        end
    
        MmaxNeg=M;
    end
   
    
    % Check if unloading
    if Di_previous*Di<0 
       theta_M0_projected=theta_previous-M_previous/ke;
    end    
    
    % Compute the tangent stiffness
    k=(M-M_previous)/(theta-theta_previous);
    if theta==theta_previous
        k=0;
    elseif M==M_previous
        k=10^-6;
    end
end