clear 
clc

% Solution of Assignement#3 with distributed 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
pts_leftCol = [];
pts_middleCol = [];
pts_rightCol = [];
pts_leftBeam = [];
pts_rightBeam = [];
nEl_column=5;
nEl_beam=5;
for i = 0:nEl_column
    y = i * (h / nEl_column);
    pts_leftCol = [pts_leftCol; 0, y];
end
for i = 0:nEl_column
    y = h - i * (h / nEl_column);
    pts_middleCol = [pts_middleCol; l1, y];
end
for i = 0:nEl_column
    y = h - i * (h / nEl_column);
    pts_rightCol = [pts_rightCol; l1 + l2, y];
end
for i = 0:nEl_beam
    x = i * (l1 / nEl_beam);
    pts_leftBeam = [pts_leftBeam; x, h];
end
for i = 0:nEl_beam
    x = l1 + i * (l2 / nEl_beam);
    pts_rightBeam = [pts_rightBeam; x, h];
end
coordinates_list = [pts_leftCol; pts_leftBeam; pts_middleCol; pts_rightBeam; pts_rightCol];
[coordinates, ~] = unique(coordinates_list, 'rows', 'stable');
nNodes = size(coordinates, 1);
nDofTot = nNodes * nDofPerNode;


%% Connectivity
coord_to_index = containers.Map('KeyType', 'char', 'ValueType', 'int32'); % Create a dictionary to map each coordinate to its index
for idx = 1:size(coordinates, 1)
    coord_to_index(mat2str(coordinates(idx, :))) = idx;
end
connectivity = [];
for i = 1:nEl_column
    connectivity = [connectivity; ...
        coord_to_index(mat2str(pts_leftCol(i, :))), ...
        coord_to_index(mat2str(pts_leftCol(i + 1, :)))];
end
for i = 1:nEl_beam
    connectivity = [connectivity; ...
        coord_to_index(mat2str(pts_leftBeam(i, :))), ...
        coord_to_index(mat2str(pts_leftBeam(i + 1, :)))];
end
for i = 1:nEl_column
    connectivity = [connectivity; ...
        coord_to_index(mat2str(pts_middleCol(i, :))), ...
        coord_to_index(mat2str(pts_middleCol(i + 1, :)))];
end
for i = 1:nEl_beam
    connectivity = [connectivity; ...
        coord_to_index(mat2str(pts_rightBeam(i, :))), ...
        coord_to_index(mat2str(pts_rightBeam(i + 1, :)))];
end
for i = 1:nEl_column
    connectivity = [connectivity; ...
        coord_to_index(mat2str(pts_rightCol(i, :))), ...
        coord_to_index(mat2str(pts_rightCol(i + 1, :)))];
end
nElements = size(connectivity, 1);


%% Member properties
E=200000; %Young's modulus
fy=355; %Yield stress
alphaS=0.03; %hardening ratio
materialInput=[E,fy,alphaS];
bc=300;
hc=300;
bb=300;
hb=700;


%% Define fiber section
nFibersInRow=1;
nFibersInCol=10;
[coordinates_fibers_columns,area_fibers_columns] = meshFiber_rectangle(hc,bc,nFibersInRow,nFibersInCol);
[coordinates_fibers_beams,area_fibers_beams] = meshFiber_rectangle(hb,bb,nFibersInRow,nFibersInCol);


%% Define quadrature rule for numerical integration
integrationRule='Gauss-Lobatto';
numIntegrPoints=5;
[integrPoints,integrWeights]=computeQuadrature(integrationRule,numIntegrPoints);


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


%% Elements properties
AllElement_data = {};  
%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=0 for X, 1 for Y, 2 for rotation
%2dModElasticBeam: '2dModElasticBeam', nodes, coordNode1, coordNode2, E, A, S22,S23,S32,S33, Ie, 
%2dDispBeamColumn: '2dDispBeamColumn', nodes, coordNode1, coordNode2,integrPoints,integrWeights, materialInput, coordinates_fibers, area_fibers, geoTransf
for i = 1:nElements
    dx = coordinates(connectivity(i,2),1) - coordinates(connectivity(i,1),1);
    dy = coordinates(connectivity(i,2),2) - coordinates(connectivity(i,1),2);
    
    % Fiber elements
    if dx == 0  % column elements
        AllElement_data{end+1} = {'2dDispBeamColumn', connectivity(i,:), coordinates(connectivity(i,1),:), coordinates(connectivity(i,2),:), integrPoints, integrWeights, materialInput, coordinates_fibers_columns, area_fibers_columns, geoTransfCols};
    else  % beam elements
        AllElement_data{end+1} = {'2dDispBeamColumn', connectivity(i,:), coordinates(connectivity(i,1),:), coordinates(connectivity(i,2),:), integrPoints, integrWeights, materialInput, coordinates_fibers_beams, area_fibers_beams, geoTransfBeams};
    end

% % Elastic elements
%     if dx == 0  % column elements
%         AllElement_data{end+1} = {'2dElasticBeam', connectivity(i,:), coordinates(connectivity(i,1),:), coordinates(connectivity(i,2),:), E, bc*hc, bc*hc^3/12, geoTransfCols};
%     else  % beam elements
%         AllElement_data{end+1} = {'2dElasticBeam', connectivity(i,:), coordinates(connectivity(i,1),:), coordinates(connectivity(i,2),:), E, bb*hb, bb*hb^3/12, geoTransfBeams};
%     end
    
end


%% EqualDof constraints matrix (master node, slave node and its constrained dofs where globalDof=0 for X, 1 for Y, 2 for rotation)
allConstraints = [];


%% 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 = {
    {nEl_column+1, [1, alpha], [2, -0.5]},
    {nEl_column+nEl_beam+1, [2, -1]},
    {2*nEl_column+2*nEl_beam+1, [2, -1]}
};
fixedBC_nodesDofs = {
    [1, 1, 2, 3],
    [2*nEl_column+nEl_beam+1, 1, 2, 3],
    [2*nEl_column+3*nEl_beam+1, 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=nDofPerNode*(nEl_column+1)-3+1; % 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
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
            [elem, 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));
        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];
        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(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 all comparison linear nonlinear
% res_case1=importdata('elastic_linGeom.txt');
% res_case2=importdata('elastic_nonlinGeom.txt');
% res_case3a=importdata('springs_linGeom.txt');
% res_case3b=importdata('fiber_linGeom.txt');
% res_case4a=importdata('springs_nonlinGeom.txt');
% res_case4b=importdata('fiber_nonlinGeom.txt');
% 
% h2=figure;
% plot(res_case1(:,1),res_case1(:,2)/1e3)
% hold on
% plot(res_case2(:,1),res_case2(:,2)/1e3)
% plot(res_case3a(:,1),res_case3a(:,2)/1e3)
% plot(res_case3b(:,1),res_case3b(:,2)/1e3)
% plot(res_case4a(:,1),res_case4a(:,2)/1e3)
% plot(res_case4b(:,1),res_case4b(:,2)/1e3)
% legend('Case 1','Case 2','Case 3a','Case 3b','Case 4a','Case 4b','Location','best')
% xlabel('u_b [mm]')
% ylabel('V_{tot} [kN]')
% xlim([0,1000])
% ylim([0,2000])
% grid on

% plot_settings_ASCE(h2)


%% Export results
% fileName=strcat('fiber_linGeom.txt');
% writematrix([v_Array(q_ctrDof,:)',F_int_Array(q_ctrDof,:)'], fileName);