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_glob = [ 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_glob*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;
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);

%% Apply reference load in one step
DeltaLambdaBar=1;

%% Determine the critical buckling load
DeltaLambda=DeltaLambdaBar;

%Update of load and displacements
DeltaV_u(freeDOfs)=Ktot(freeDOfs,freeDOfs)\-F_unb(freeDOfs);
DeltaV_f(freeDOfs)=Ktot(freeDOfs,freeDOfs)\F_ext(freeDOfs);
lambda=lambda+DeltaLambda;
DeltaV=DeltaV_u+DeltaLambda*DeltaV_f;
v=v+DeltaV;

% Compute stiffness matrix and resisting force vector
u=T*v;

[uBar]=computeTransfoLocalToBasic_corot(l,u);

qBar=Kbar*uBar;

[L,Kg]=computeTransfoBasicToLocal_corot(l,u,qBar);

%% Resoluion of the eigenvalue probles
Ke_glob=T'*L'*Kbar*L*T;
Kg_glob=T'*Kg*T;
numEigenValues = min(7,size(Kg_glob(freeDOfs,freeDOfs),1)); % Adjust as needed based on your problem
[V, k] = eigs(Kg_glob(freeDOfs, freeDOfs), Ke_glob(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=Pcr/1000












%% 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