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=2;


%% 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='linear';
geoTransfBeam='linear';
% geoTransfCols='corotational';
% geoTransfBeam='linear';

%% 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);
KTot_global=KMaterial_global;

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

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.1;
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);

%% Integrator Force or displacement control integrator FIXED STEP SIZE
integrator="forceControl";
lambda_max=4803400*0.5;
nIncrements=500;
DeltaLambdaBar=lambda_max/nIncrements;
tol=1e-4;
nIterMax=1000;

% integrator='displacementControl'
% uMax=1350
% nIncrements=500
% DeltaVBar=uMax/nIncrements
% q_ctrDof=nDof*(nEl_column+1)-2; % Control DOF
% tol=1e-4
% nIterMax=1000

if integrator=="forceControl"
    aIntegrator=zeros(nDofTot,1);
    bIntegrator=1;
    cIntegrator=[DeltaLambdaBar,0];
elseif integrator=="displacementControl"
    aIntegrator=zeros(nDofTot,1);
    aIntegrator(q_ctrDof)=1;
    bIntegrator=0;
    cIntegrator=[DeltaVBar,0];
end
    


%% Solve the system
for n=1:nIncrements
    converged=0;
    i=1;
    
    % Iteration
    while converged==0
        DeltaV_u(freeDofs) = KTot_global(freeDofs, freeDofs) \ (-F_unb(freeDofs));
        DeltaV_f(freeDofs) = KTot_global(freeDofs, freeDofs) \ F_ext(freeDofs);
        if i == 1
            DeltaLambda = (cIntegrator(1) - dot(aIntegrator, DeltaV_u)) / (dot(aIntegrator, DeltaV_f) + bIntegrator);
        else
            DeltaLambda = (cIntegrator(2) - dot(aIntegrator, DeltaV_u)) / (dot(aIntegrator, DeltaV_f) + bIntegrator);
        end
        
        lambdaIntegrator = lambdaIntegrator + DeltaLambda;
        DeltaV = DeltaV_u + DeltaLambda * DeltaV_f;
        v = v + DeltaV;
        
        % Element state determination
        F_int = zeros(nDofTot, 1);
        KTot_global = zeros(nDofTot);
        QLocal_elem_step = [];
        
        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);
            
            QLocal_elem_step = [QLocal_elem_step,QGlobal_elem];
            
            % Assemble element state determination results
            dof_elem = numEquations{elemIdx};
            F_int(dof_elem) = F_int(dof_elem) + QGlobal_elem;
            KTot_global(dof_elem, dof_elem) = KTot_global(dof_elem, dof_elem) + KMaterialGlobal_elem+KGeomGlobal_elem;
        end
        
        % 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_Array=[v_Array,v];
            F_int_Array=[F_int_Array,F_int];
            lambda_Vector=[lambda_Vector,lambdaIntegrator];
            QLocal_elem_list(n,:,:)=[QLocal_elem_step];
        end
        
        if i==nIterMax && converged==0
            error('Failled to converge')
        end
        i=i+1;
    end
end

%% Plot the load-displacement results
% h1=figure;
% plot(v_Array(nDofPerNode*(nEl_column+1)-2,:),F_int_Array(nDofPerNode*(nEl_column+1)-2,:)/1e3,'-k')
% xlabel('u_b [mm]')
% ylabel('V_tot [kN]')
% grid on

% plot_settings_ASCE(h1)


%% Plot the internal forces
%plotInternalForces2dBeam(AllElement_data,squeeze(QLocal_elem_list(end,:,:)),'internalForces')


%% Plot comparison linear nonlinear
res_LG_LM=importdata('forceControl_linear.txt');
res_NlG_LM=importdata('forceControl_corotational.txt');

h2=figure;
plot(res_LG_LM(:,1),res_LG_LM(:,2)/1e3)
hold on
plot(res_NlG_LM(:,1),res_NlG_LM(:,2)/1e3)
legend('Linear geom','Nonlinear geom','Location','best')
xlabel('u_b [mm]')
ylabel('V_{tot} [kN]')
grid on

% plot_settings_ASCE(h2)


%% Export results
% fileName=strcat(integrator,'_',geoTransfCols,'.txt')
% writematrix([v_Array(nDofPerNode*(nEl_column+1)-2,:)',F_int_Array(nDofPerNode*(nEl_column+1)-2,:)'], fileName);