clear 
clc

% Solution of Assignement#3 with concentrated plasticity

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


%% Frame geometry
h=7*1e3;
l1=6*1e3;
l2=12*1e3;


%% Node coordinates
node0 = [0, 0];
node1 = [0, 0];
node2 = [0, h];
node3 = [0, h];
node4 = [0, h];
node5 = [l1, h];
node6 = [l1, h];
node7 = [l1, h];
node8 = [l1, 0];
node9 = [l1, 0];
node10 = [l1, h];
node11 = [l1 + l2, h];
node12 = [l1 + l2, h];
node13 = [l1 + l2, h];
node14 = [l1 + l2, 0];
node15 = [l1 + l2, 0];

allNodes = [
    node0;
    node1;
    node2;
    node3;
    node4;
    node5;
    node6;
    node7;
    node8;
    node9;
    node10;
    node11;
    node12;
    node13;
    node14;
    node15
];
coordinates = allNodes;
nNodes = size(coordinates, 1);


%% Connectivity
nodes_elem1 = [1, 0];
nodes_elem2 = [1, 2];
nodes_elem3 = [2, 3];
nodes_elem4 = [4, 3];
nodes_elem5 = [4, 5];
nodes_elem6 = [5, 6];
nodes_elem7 = [7, 6];
nodes_elem8 = [7, 8];
nodes_elem9 = [8, 9];
nodes_elem10 = [10, 6];
nodes_elem11 = [10, 11];
nodes_elem12 = [11, 12];
nodes_elem13 = [13, 12];
nodes_elem14 = [13, 14];
nodes_elem15 = [14, 15];

connectivity = [
    nodes_elem1;
    nodes_elem2;
    nodes_elem3;
    nodes_elem4;
    nodes_elem5;
    nodes_elem6;
    nodes_elem7;
    nodes_elem8;
    nodes_elem9;
    nodes_elem10;
    nodes_elem11;
    nodes_elem12;
    nodes_elem13;
    nodes_elem14;
    nodes_elem15
]+1;
nElements = size(connectivity, 1);


%% Member properties
E=200000; %Young's modulus
fy=355; %Yield stress
bc=300;
hc=300;
Ic=bc*hc^3/12;
Ac=bc*hc;
bb=300;
hb=700;
Ib=bb*hb^3/12;
Ab=bb*hb;
thetaP_mem=0.02;
thetaPc_mem=0.05;
% Columns
ke_mem_col=3*E*Ic/(h/2);
My_col=Ic/(hc/2)*fy;
Mu_col=1.1*My_col;
thetaY_mem_col=My_col/ke_mem_col;
thetaC_mem_col=thetaP_mem+thetaY_mem_col;
thetaU_mem_col=thetaPc_mem+thetaC_mem_col;
as_mem_col=(Mu_col-My_col)/(thetaP_mem*ke_mem_col);
apc_mem_col=-Mu_col/(thetaPc_mem*ke_mem_col);
% Left beam
ke_mem_leftBeam=3*E*Ib/(l1/2);
My_leftBeam=Ib/(hb/2)*fy;
Mu_leftBeam=1.1*My_leftBeam;
thetaY_mem_leftBeam=My_leftBeam/ke_mem_leftBeam;
thetaC_mem_leftBeam=thetaP_mem+thetaY_mem_leftBeam;
thetaU_mem_leftBeam=thetaPc_mem+thetaC_mem_leftBeam;
as_mem_leftBeam=(Mu_leftBeam-My_leftBeam)/(thetaP_mem*ke_mem_leftBeam);
apc_mem_leftBeam=-Mu_leftBeam/(thetaPc_mem*ke_mem_leftBeam);
% Right beam
ke_mem_rightBeam=3*E*Ib/(l2/2);
My_rightBeam=Ib/(hb/2)*fy;
Mu_rightBeam=1.1*My_rightBeam;
thetaY_mem_rightBeam=My_rightBeam/ke_mem_rightBeam;
thetaC_mem_rightBeam=thetaP_mem+thetaY_mem_rightBeam;
thetaU_mem_rightBeam=thetaPc_mem+thetaC_mem_rightBeam;
as_mem_rightBeam=(Mu_rightBeam-My_rightBeam)/(thetaP_mem*ke_mem_rightBeam);
apc_mem_rightBeam=-Mu_rightBeam/(thetaPc_mem*ke_mem_rightBeam);


%% Spring and elastic beam-column element properties
nSpring=10;
S22mod=(6*(2*nSpring + 1))/(3*nSpring + 2);
S23mod=(6*(nSpring + 1))/(3*nSpring + 2);
S32mod=(6*(nSpring + 1))/(3*nSpring + 2);
S33mod=(12*nSpring + 6)/(3*nSpring + 2);

% Columns
Ie_col=(nSpring+1)/nSpring*Ic;
ke_s_col=nSpring*3*E*Ie_col/(h/2);
ks_s_col=as_mem_col/(1+nSpring*(1-as_mem_col))*ke_s_col;
kpc_s_col=apc_mem_col/(1+nSpring*(1-apc_mem_col))*ke_s_col;
thetaY_s_col=My_col/ke_s_col;
thetaP_s_col  = (Mu_col-My_col)/ks_s_col; %Pre-capping Plastic rotation of the spring
thetaPc_s_col = (0-Mu_col)/(kpc_s_col); % Post-capping plastic rotation of the spring
thetaC_s_col=thetaP_s_col+thetaY_s_col;
thetaU_s_col=thetaPc_s_col+thetaC_s_col;
% Left beam
Ie_leftBeam=(nSpring+1)/nSpring*Ib;
ke_s_leftBeam=nSpring*3*E*Ie_leftBeam/(l1/2);
ks_s_leftBeam=as_mem_leftBeam/(1+nSpring*(1-as_mem_leftBeam))*ke_s_leftBeam;
kpc_s_leftBeam=apc_mem_leftBeam/(1+nSpring*(1-apc_mem_leftBeam))*ke_s_leftBeam;
thetaY_s_leftBeam=My_leftBeam/ke_s_leftBeam;
thetaP_s_leftBeam  = (Mu_leftBeam-My_leftBeam)/ks_s_leftBeam; %Pre-capping Plastic rotation of the spring
thetaPc_s_leftBeam = (0-Mu_leftBeam)/(kpc_s_leftBeam); % Post-capping plastic rotation of the spring
thetaC_s_leftBeam=thetaP_s_leftBeam+thetaY_s_leftBeam;
thetaU_s_leftBeam=thetaPc_s_leftBeam+thetaC_s_leftBeam;
% Right beam
Ie_rightBeam=(nSpring+1)/nSpring*Ib;
ke_s_rightBeam=nSpring*3*E*Ie_rightBeam/(l2/2);
ks_s_rightBeam=as_mem_rightBeam/(1+nSpring*(1-as_mem_rightBeam))*ke_s_rightBeam;
kpc_s_rightBeam=apc_mem_rightBeam/(1+nSpring*(1-apc_mem_rightBeam))*ke_s_rightBeam;
thetaY_s_rightBeam=My_rightBeam/ke_s_rightBeam;
thetaP_s_rightBeam  = (Mu_rightBeam-My_rightBeam)/ks_s_rightBeam; %Pre-capping Plastic rotation of the spring
thetaPc_s_rightBeam = (0-Mu_rightBeam)/(kpc_s_rightBeam); % Post-capping plastic rotation of the spring
thetaC_s_rightBeam=thetaP_s_rightBeam+thetaY_s_rightBeam;
thetaU_s_rightBeam=thetaPc_s_rightBeam+thetaC_s_rightBeam;


%% Initialize parameters for spring constitutive law
theta_previous=0;
M_previous=0;
MmaxPos_col=My_col;
MmaxNeg_col=-My_col;
MmaxPos_leftBeam=My_leftBeam;
MmaxNeg_leftBeam=-My_leftBeam;
MmaxPos_rightBeam=My_rightBeam;
MmaxNeg_rightBeam=-My_rightBeam;
yieldFlag_Pos=0;
cappingFlag_Pos=0;
yieldFlag_Neg=0;
cappingFlag_Neg=0;
reversalFlag=0;
residualFlag=0;
theta_M0_currentPos=0;
theta_M0_currentNeg=0;
theta_M0_projected=0;
Di_previous=0;

%% Fill for each spring
column_springs_MatProp_Input = zeros(1, 22);
column_springs_MatProp_Input(1) = theta_previous;
column_springs_MatProp_Input(2) = M_previous;
column_springs_MatProp_Input(3) = theta_M0_currentPos;
column_springs_MatProp_Input(4) = theta_M0_currentNeg;
column_springs_MatProp_Input(5) = theta_M0_projected;
column_springs_MatProp_Input(6) = ke_s_col;
column_springs_MatProp_Input(7) = ks_s_col;
column_springs_MatProp_Input(8) = kpc_s_col;
column_springs_MatProp_Input(9) = thetaY_s_col;
column_springs_MatProp_Input(10) = thetaC_s_col;
column_springs_MatProp_Input(11) = thetaU_s_col;
column_springs_MatProp_Input(12) = My_col;
column_springs_MatProp_Input(13) = Mu_col;
column_springs_MatProp_Input(14) = MmaxPos_col;
column_springs_MatProp_Input(15) = MmaxNeg_col;
column_springs_MatProp_Input(16) = yieldFlag_Pos;
column_springs_MatProp_Input(17) = cappingFlag_Pos;
column_springs_MatProp_Input(18) = yieldFlag_Neg;
column_springs_MatProp_Input(19) = cappingFlag_Neg;
column_springs_MatProp_Input(20) = reversalFlag;
column_springs_MatProp_Input(21) = residualFlag;
column_springs_MatProp_Input(22) = Di_previous;
column_springs_MatProp_Output = column_springs_MatProp_Input;

leftBeam_springs_MatProp_Input = zeros(1, 22);
leftBeam_springs_MatProp_Input(1) = theta_previous;
leftBeam_springs_MatProp_Input(2) = M_previous;
leftBeam_springs_MatProp_Input(3) = theta_M0_currentPos;
leftBeam_springs_MatProp_Input(4) = theta_M0_currentNeg;
leftBeam_springs_MatProp_Input(5) = theta_M0_projected;
leftBeam_springs_MatProp_Input(6) = ke_s_leftBeam;
leftBeam_springs_MatProp_Input(7) = ks_s_leftBeam;
leftBeam_springs_MatProp_Input(8) = kpc_s_leftBeam;
leftBeam_springs_MatProp_Input(9) = thetaY_s_leftBeam;
leftBeam_springs_MatProp_Input(10) = thetaC_s_leftBeam;
leftBeam_springs_MatProp_Input(11) = thetaU_s_leftBeam;
leftBeam_springs_MatProp_Input(12) = My_leftBeam;
leftBeam_springs_MatProp_Input(13) = Mu_leftBeam;
leftBeam_springs_MatProp_Input(14) = MmaxPos_leftBeam;
leftBeam_springs_MatProp_Input(15) = MmaxNeg_leftBeam;
leftBeam_springs_MatProp_Input(16) = yieldFlag_Pos;
leftBeam_springs_MatProp_Input(17) = cappingFlag_Pos;
leftBeam_springs_MatProp_Input(18) = yieldFlag_Neg;
leftBeam_springs_MatProp_Input(19) = cappingFlag_Neg;
leftBeam_springs_MatProp_Input(20) = reversalFlag;
leftBeam_springs_MatProp_Input(21) = residualFlag;
leftBeam_springs_MatProp_Input(22) = Di_previous;
leftBeam_springs_MatProp_Output = leftBeam_springs_MatProp_Input;

rightBeam_springs_MatProp_Input = zeros(1, 22);
rightBeam_springs_MatProp_Input(1) = theta_previous;
rightBeam_springs_MatProp_Input(2) = M_previous;
rightBeam_springs_MatProp_Input(3) = theta_M0_currentPos;
rightBeam_springs_MatProp_Input(4) = theta_M0_currentNeg;
rightBeam_springs_MatProp_Input(5) = theta_M0_projected;
rightBeam_springs_MatProp_Input(6) = ke_s_rightBeam;
rightBeam_springs_MatProp_Input(7) = ks_s_rightBeam;
rightBeam_springs_MatProp_Input(8) = kpc_s_rightBeam;
rightBeam_springs_MatProp_Input(9) = thetaY_s_rightBeam;
rightBeam_springs_MatProp_Input(10) = thetaC_s_rightBeam;
rightBeam_springs_MatProp_Input(11) = thetaU_s_rightBeam;
rightBeam_springs_MatProp_Input(12) = My_rightBeam;
rightBeam_springs_MatProp_Input(13) = Mu_rightBeam;
rightBeam_springs_MatProp_Input(14) = MmaxPos_rightBeam;
rightBeam_springs_MatProp_Input(15) = MmaxNeg_rightBeam;
rightBeam_springs_MatProp_Input(16) = yieldFlag_Pos;
rightBeam_springs_MatProp_Input(17) = cappingFlag_Pos;
rightBeam_springs_MatProp_Input(18) = yieldFlag_Neg;
rightBeam_springs_MatProp_Input(19) = cappingFlag_Neg;
rightBeam_springs_MatProp_Input(20) = reversalFlag;
rightBeam_springs_MatProp_Input(21) = residualFlag;
rightBeam_springs_MatProp_Input(22) = Di_previous;
rightBeam_springs_MatProp_Output = rightBeam_springs_MatProp_Input;


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


%% Elements properties
%2dTruss: '2dTruss', nodes, coordNode1, coordNode2, E, A, geoTransf
%2dElasticBeam: '2dElasticBeam', nodes, coordNode1, coordNode2, E, A, I, geoTransf
%spring: 'spring', nodes, globalDof, springInput, springOutput       where globalDof=2 for X, 2 for Y, 3 for rotation
%2dModElasticBeam: '2dModElasticBeam', nodes, coordNode1, coordNode2, E, A, S22,S23,S32,S33, Ie, geoTransf

% Left Column
elem1 = {'spring', connectivity(1, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
elem2 = {'2dModElasticBeam', connectivity(2, :), coordinates(connectivity(2, 1), :), coordinates(connectivity(2, 2), :), ...
         E, Ac, S22mod, S23mod, S32mod, S33mod, Ie_col, geoTransfCols};
elem3 = {'spring', connectivity(3, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
% Left Beam
elem4 = {'spring', connectivity(4, :), 3, leftBeam_springs_MatProp_Input, leftBeam_springs_MatProp_Output};
elem5 = {'2dModElasticBeam', connectivity(5, :), coordinates(connectivity(5, 1), :), coordinates(connectivity(5, 2), :), ...
         E, Ab, S22mod, S23mod, S32mod, S33mod, Ie_leftBeam, geoTransfBeam};
elem6 = {'spring', connectivity(6, :), 3, leftBeam_springs_MatProp_Input, leftBeam_springs_MatProp_Output};
% Middle Column
elem7 = {'spring', connectivity(7, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
elem8 = {'2dModElasticBeam', connectivity(8, :), coordinates(connectivity(8, 1), :), coordinates(connectivity(8, 2), :), ...
         E, Ac, S22mod, S23mod, S32mod, S33mod, Ie_col, geoTransfCols};
elem9 = {'spring', connectivity(9, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
% Right Beam
elem10 = {'spring', connectivity(10, :), 3, rightBeam_springs_MatProp_Input, rightBeam_springs_MatProp_Output};
elem11 = {'2dModElasticBeam', connectivity(11, :), coordinates(connectivity(11, 1), :), coordinates(connectivity(11, 2), :), ...
          E, Ab, S22mod, S23mod, S32mod, S33mod, Ie_rightBeam, geoTransfBeam};
elem12 = {'spring', connectivity(12, :), 3, rightBeam_springs_MatProp_Input, rightBeam_springs_MatProp_Output};
% Right Column
elem13 = {'spring', connectivity(13, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
elem14 = {'2dModElasticBeam', connectivity(14, :), coordinates(connectivity(14, 1), :), coordinates(connectivity(14, 2), :), ...
          E, Ac, S22mod, S23mod, S32mod, S33mod, Ie_col, geoTransfCols};
elem15 = {'spring', connectivity(15, :), 3, column_springs_MatProp_Input, column_springs_MatProp_Output};
% Combine all elements into a single cell array
AllElement_data = {elem1, elem2, elem3, elem4, elem5, elem6, elem7, elem8, ...
                   elem9, elem10, elem11, elem12, elem13, elem14, elem15};
               
%%  EqualDof constraints matrix 
%(master node, slave node and its constrained dofs where globalDof=1 for X, 2 for Y, 3 for rotation)
constraint1=[0,1,0,1]+1;
constraint2=[2,3,0,1]+1;
constraint3=[2,4,0,1]+1;
constraint4=[5,6,0,1]+1;
constraint5=[5,7,0,1]+1;
constraint6=[8,9,0,1]+1;
constraint7=[5,10,0,1]+1;
constraint8=[11,12,0,1]+1;
constraint9=[11,13,0,1]+1;
constraint10=[14,15,0,1]+1;
allConstraints = {constraint1;constraint2;constraint3;constraint4;constraint5;constraint6;constraint7;constraint8;constraint9;constraint10};


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


%% Definition of the boundary conditions
alpha=0.1;


%% Apply direct method for constraint enforcement
F_ext_nodesDofs = {
    {4, [1, alpha], [2, -0.5]},
    {7, [2, -1]},
    {13, [2, -1]}
};
fixedBC_nodesDofs = {
    [1, 1, 2, 3],
    [10, 1, 2, 3],
    [16, 1, 2, 3]
};
[numEquations, F_ext, freeDofs, fixedDofs]=directConstraintEnformcement(nDofPerNode,numEquations_noConstraints, F_ext_nodesDofs,fixedBC_nodesDofs,allConstraints);
nDofTot = max(cellfun(@max, numEquations));

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

%% 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=1000;
nIncrements=500;
DeltaVBar=uMax/nIncrements;
q_ctrDof=5; % Control DOF
tol=1e-2;
nIterMax=5000;

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
breakFlag=0;
for n=1:nIncrements
    converged=0;
    test=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
            [elem, QGlobal_elem, KMaterialGlobal_elem, KGeomGlobal_elem, QLocal_elem] = elemStateDetermin(elemIdx, elem, numEquations, v);
            AllElement_data{elemIdx}=elem;
            
            %             QLocal_elem_step = [QLocal_elem_step,QGlobal_elem]; %Todo: fix this for the case of springs
            
            % 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));
        test=norm(F_unb(freeDofs))/norm(F_ext_tot(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];
            %Update parameters for constitutive models
            for i = 1:length(AllElement_data)
                elem = AllElement_data{i};  
                if strcmp(elem{1}, 'spring')  
                    elem{4} = elem{5};  
                    AllElement_data{i}=elem;
                end
            end
        end
        
        if i==nIterMax && converged==0
            %error('Failled to converge')
            breakFlag=1;
            break;
        end
        i=i+1;
    end
    if breakFlag==1
       break 
    end
end

%% Plot the load-displacement results
h1=figure;
plot(v_Array(q_ctrDof,:),F_int_Array(q_ctrDof,:)/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('springs_nonlinGeom.txt');
% writematrix([v_Array(q_ctrDof,:)',F_int_Array(q_ctrDof,:)'], fileName);