clear 
clc

% Solution of Assignement#2

%% Dimension and degrees of freedom of the problem
nDim = 2;
nDofPerNode = 3; % nDof per node
nNodesPerElement = 2;

%% Material properties
E = 200000;

%% Frame properties
h=7*1e3;
l1=6*1e3;
l2=12*1e3;
Ab=16000;
Ib=1.186*10^9;
Ac=8600;
Ic=1.032*10^8;
nEl_column=50;


%% Node coordinates
pts_leftCol = zeros(nEl_column+1,2);
pts_middleCol = zeros(nEl_column+1,2);
pts_rightCol = zeros(nEl_column+1,2);
for i = 1:nEl_column+1
    y = (i-1) * (h / nEl_column);
    pts_leftCol(i,:) = [0, y];
    pts_middleCol(i,:) = [l1, y];
    pts_rightCol(i,:) = [l1 + l2, y];
end
coordinates = [pts_leftCol; pts_middleCol; pts_rightCol];
nNodes = size(coordinates, 1);
nDofTot = nNodes * nDofPerNode;

%% Connectivity
elems_leftCol = zeros(nEl_column,2);
elems_middleCol = zeros(nEl_column,2);
elems_rightCol = zeros(nEl_column,2);
for i = 1:nEl_column
    elems_leftCol(i,:) = [i, i+1];
    elems_middleCol(i,:) = [i + nEl_column + 1, i + nEl_column + 2];
    elems_rightCol(i,:) = [i + 2 * nEl_column + 2, i + 2 * nEl_column + 3];
end
elems_beam = [nEl_column + 1, 2 * (nEl_column + 1);
              2 * (nEl_column + 1), 3 * (nEl_column + 1)];
connectivity = [elems_leftCol; elems_middleCol; elems_rightCol; elems_beam];
% disp(coordinates)
% disp(connectivity)
nElements = size(connectivity, 1);

%% Geometric transformation local <-> basic reference frame
geoTransfCols='corotational';
geoTransfBeam='corotational';

%% Elements properties
AllElement_data = {};  
%2dTruss: '2dTruss', nodes, coordNode1, coordNode2, E, A, geoTransf
%#2dBeam: '2dBeam', nodes, coordNode1, coordNode2, E, A, I, geoTransf
for i = 1:3*nEl_column
    elem = {'2dElasticBeam', connectivity(i, :), coordinates(connectivity(i, 1), :), coordinates(connectivity(i, 2), :), E, Ac, Ic, geoTransfCols};
    AllElement_data{end+1} = elem;
end
% Loop for beam elements
for i = 1:2
    elem = {'2dElasticBeam', connectivity(3*nEl_column + i, :), coordinates(connectivity(3*nEl_column + i, 1), :), coordinates(connectivity(3*nEl_column + i, 2), :), E, Ab, Ib, geoTransfBeam};
    AllElement_data{end+1} = elem;
end

%% Equation number matrix
numEquations=computeEquationNumber(nDofPerNode,AllElement_data);

%% Assemble global stiffness matrix
KMaterial_global=assembleInitialGlobalMaterialStiffnessMatrix(nDofTot,numEquations,AllElement_data);

%% Initialize variables
v_Array=[];
F_int_Array=[];
lambda_Vector=[];

F_int=zeros(nDofTot,1);
lambdaIntegrator=0;
v=zeros(nDofTot,1);
F_unb=zeros(nDofTot,1);
F_ext_tot=zeros(nDofTot,1);

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

%% Definition of the boundary conditions
alpha = 0.0;
F_ext = zeros(nDofTot, 1);
F_ext(nDofPerNode * (nEl_column + 1) - 2) = alpha;
F_ext(nDofPerNode * (nEl_column + 1) - 1) = -0.5;
F_ext(2 * nDofPerNode * (nEl_column + 1) - 1) = -1.0;
F_ext(3 * nDofPerNode * (nEl_column + 1) - 1) = -1.0;
allDofs = (1:nDofTot)';
fixedDofs = [1; 2; 3;
             nDofPerNode * (nEl_column + 1)+1;
             nDofPerNode * (nEl_column + 1) + 2;
             nDofPerNode * (nEl_column + 1) + 3;
             2 * nDofPerNode * (nEl_column + 1)+1;
             2 * nDofPerNode * (nEl_column + 1) + 2;
             2 * nDofPerNode * (nEl_column + 1) + 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) = KMaterial_global(freeDofs, freeDofs) \ (-F_unb(freeDofs));
DeltaV_f(freeDofs) = KMaterial_global(freeDofs, freeDofs) \ F_ext(freeDofs);

lambdaIntegrator = lambdaIntegrator + DeltaLambda;
DeltaV = DeltaV_u + DeltaLambda * DeltaV_f;
v = v + DeltaV;

% Element state determination
F_int = zeros(nDofTot, 1);
KMaterial_global = zeros(nDofTot, nDofTot);
KGeom_global = zeros(nDofTot, nDofTot);

for elemIdx = 1:numel(AllElement_data)
    elem = AllElement_data{elemIdx};
    
    % Perform element state determination
    [QGlobal_elem, KMaterialGlobal_elem, KGeomGlobal_elem, QLocal_elem] = elemStateDetermin(elemIdx, elem, numEquations, v);
    
    % Assemble element state determination results
    dof_elem = numEquations{elemIdx};
    F_int(dof_elem) = F_int(dof_elem) + QGlobal_elem;
    KMaterial_global(dof_elem, dof_elem) = KMaterial_global(dof_elem, dof_elem) + KMaterialGlobal_elem;
    KGeom_global(dof_elem, dof_elem) = KGeom_global(dof_elem, dof_elem) + KGeomGlobal_elem;
end

%% Resolution of the eigenvalue problem
num_eigenvalues = min(7, numel(freeDofs));

Kg_sub = KGeom_global(freeDofs, freeDofs);
Ke_sub = KMaterial_global(freeDofs, freeDofs);

[eigenvectors, eigenvalues] = eigs(Kg_sub, Ke_sub, num_eigenvalues, 'largestabs');
eigenvalues = 1 ./ diag(eigenvalues);
ind1 = abs(imag(eigenvalues))<10^-4;
B = eigenvalues(ind1);
V2 = eigenvectors(:, ind1);

[Pcr, Pcr_index] = min(abs(B));

% Eigenvector corresponding to the critical buckling load
V2 = V2(:, Pcr_index);


%% Print the results
fprintf('Pcr = %.3f kN\n', Pcr / 1e3);

%% Plot deformed structure
u_global = zeros(nDofTot, 1);
u_global(freeDofs) = V2;
plotDeformedStructure2d(nDofPerNode,coordinates,connectivity,abs(u_global),1e7,'deformedStructure');




test=1;