clear all
clc

%This script test the corotational approach for stability

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

nMax=10;
ratioVector=zeros(nMax,1);
for i=1:nMax
    nb_el=i;
    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)];
    fixeDofs=[1 2  (3*(nb_el+1)-1) ];
    freeDOfs=setdiff(allDofs,fixeDofs);
    
    %% Resolution f=k*u in global
    U=zeros(3*(size(element, 2)+1),1);
    U(freeDOfs) = K_e(freeDOfs, freeDOfs) \ F(freeDOfs);
    F(fixeDofs)=K_e(fixeDofs,:)*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 = min(7,size(K_g(freeDOfs,freeDOfs),1)); % 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);
  
    Pcr_theory=pi^2*E*I/l_col^2;
    ratio=Pcr/Pcr_theory;
    
    ratioVector(i)=ratio;
    
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;
plot(1:nMax,ratioVector,'-k')
xlabel('nb elements [-]')
ylabel('P_{cr}/P_E [-]')
grid on

plot_settings_ASCE(h2)