clear
clc

%This script test the corotational approach for stability

%% Equation number
l_col=3*1000;      %total length of the beam column (4000mm)
nb_el=4;               %number of elements

element=[1:1:nb_el];
connectivity=zeros(numel(element),2);
for e=element                %connectivity matrix
    connectivity(e,1)=e;
    connectivity(e,2)=e+1;
end
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
 
 
 %% Local elastic stiffness matrix
E=200000;       %steel properties

% %  Square section
% b=100;
% A=b^2;
% I=b^4/12;

%HEA320
% Iz=229.3*10^6;
% Iy=69.9*10^6;
% Ip=Iz+Iy;
% Iw=1514579*10^6;
% J=1.09*10^6;
I=229.3*10^6;
A=12400;

% %IPE80
% Iz=0.801*10^6;
% Iy=0.085*10^6;
% Ip=Iz+Iy;
% Iw=1.18*10^8;
% J=0.0067*10^6;
% I=0.801*10^6;
% A=764;

%IPE100
% Iz=1.71*10^6;
% Iy=0.159^6;
% Ip=Iz+Iy;
% Iw=8956.8;
% J=0.0115*10^6;
% A=1030;

%IPE120
% Iz=3.18*10^6;
% Iy=0.277*10^6;
% Ip=Iz+Iy;
% Iw=13897;
% J=0.0169*10^6;
% A=1320;

%IPE140
% Iz=5.41*10^6;
% Iy=0.449*10^6;
% Ip=Iz+Iy;
% Iw=1.9814*10^9;
% J=0.024*10^6;
% A=1640;

% %IPE200
% Iz=19.4*10^6;
% Iy=1.42*10^6;
% Ip=Iz+Iy;
% Iw=1.3*10^10;
% J=0.0685*10^6;
% A=2850;

%IPE400
% Iz=231.3*10^6;
% Iy=13.2*10^6;
% Ip=Iz+Iy;
% Iw=4.9005*10^11;
% J=0.504*10^6;
% A=8450;

%IPE600
% Iz=920.8*10^6;
% Iy=33.9*10^6;
% Ip=Iz+Iy;
% Iw=2.859*10^12;
% J=1.65*10^6;
% A=15600;

%% Local Elastic stiffness matrix
L=l_col/nb_el;
EA = E * A;   % Axial rigidity
EI = E * I;   % Flexural rigidity
k_loc = [ EA/L,         0,          0,        -EA/L,         0,           0;
       0,   12*EI/L^3,   6*EI/L^2,         0, -12*EI/L^3,   6*EI/L^2;
       0,    6*EI/L^2,    4*EI/L,         0,  -6*EI/L^2,    2*EI/L;
     -EA/L,         0,          0,         EA/L,         0,           0;
       0,  -12*EI/L^3,  -6*EI/L^2,         0,  12*EI/L^3,  -6*EI/L^2;
       0,    6*EI/L^2,    2*EI/L,         0,  -6*EI/L^2,    4*EI/L];

%% Global elastric stiffness matrix
K_e = zeros(3*(size(element, 2)+1));
for e = 1:size(connectivity, 1)             %Assembling of the global elastric stiffness matrix using the equation number matrix
    ddl = numEq(e, :);
    K_e(ddl, ddl) = K_e(ddl, ddl) + k_loc;
end

%% Definition of the support and loading conditions
% Euler column
F=zeros(3*(size(element, 2)+1),1);
F(end-2,1)=-1;
allDofs=[1:1:3*(size(element, 2)+1)];
fixedDofs=[1 2  3 ];
freeDofs=setdiff(allDofs,fixedDofs);

%% Add the spring to elastic stiffness matrix
nPts=1000;
alphaVector=logspace(-4,3,nPts);

KVector=zeros(size(alphaVector));
for i=1:nPts
    kSpring=alphaVector(i)*3*E*I/l_col^3;
    K_e(end-1,end-1)=K_e(end-1,end-1)+kSpring;
    
    %% Resolution f=k*u en global
    U=zeros(3*(size(element, 2)+1),1);
    U(freeDofs) = K_e(freeDofs, freeDofs) \ F(freeDofs);
    F(fixedDofs)=K_e(fixedDofs,:)*U;
    
    %% Resolution of f=k*u for local internal forces and computation of geometric stiffness matrix
    K_g= zeros(3*(size(element, 2)+1));
    for e=1:size(element, 2)
        ddls=numEq(e,:);
        u_ele=U(ddls);          %we find the displacements of the considered beam element
        f_int=k_loc*u_ele;  %computation of internal forces
        
        %Geometric stiffness matrix
        Fx2=-f_int(4);
        kg_loc = (Fx2 / L) * [
            1,    0,    0,   -1,    0,    0;
            0,    6/5,  L/10, 0,    -6/5, L/10;
            0,    L/10, (2*L^2)/15, 0,    -L/10, -L^2/30;
            -1,    0,    0,    1,    0,    0;
            0,   -6/5, -L/10, 0,    6/5,  -L/10;
            0,    L/10, -L^2/30, 0, -L/10, (2*L^2)/15
            ];
        ddl = numEq(e, :);
        K_g(ddl, ddl) = (K_g(ddl, ddl) + kg_loc);   %global geoemtric stiffness matrix
    end
    
    %% Resoluion of the eigenvalue probles
    numEigenValues = 7; % Adjust as needed based on your problem
    [V, k] = eigs(K_g(freeDofs, freeDofs), K_e(freeDofs, freeDofs), numEigenValues, 'largestabs');
    
    k=diag(k);
    lambda=1./k;
    ind1 = abs(imag(lambda))==0;
    B=lambda(ind1);
    V2 = V(:, ind1);
    [Pcr,index_mode]=min(abs(B));
    V2 = V2(:, index_mode);
    
    K=sqrt(pi^2*E*I/(Pcr*l_col^2));
    KVector(i)=K;
    
end

test=1;

%% Plot of the results
% deformedShape=zeros(size(U));
% deformedShape(freeDofs)=V2;
% allYdisp=deformedShape(2:3:end);
% allLoc=linspace(0,l_col,nb_el+1);
% 
% h1=figure;
% plot(allYdisp,allLoc)


h2=figure;
semilogx(alphaVector,KVector,'-k')
xlabel('\alpha [-]')
ylabel('K [-]')
grid on

plot_settings_ASCE(h2)