%
%                      PGD Code for Dynamic Problem
%                    D. Gonz‡lez, I. Alfaro, E. Cueto
%                        Universidad de Zaragoza
%                          AMB-I3A Dec 2015
%
clear all; close all; clc;
%
% VARIABLES
%
global E nu coords tet
Modulus = 10000.0; % Force modulus.
cooru = linspace(-5E1,5E1,300); % Discretization for displacement field.
coorv = linspace(-5E+2,5E+2,300);% Discretization for velocity field.
coora = linspace(-1E+3,1E+3,300); % Discretization for acceleration field.
deltat = 0.00125; coort = 0:deltat:2; % Time discretization.
TOL = 1.0E-04; % Tolerance.
num_max_iter = 15; % # of summands of the approach.
E = 2E11; nu = 0.3; Rho = 2.5E+04; % Material.
deltatA = 0.00125; % Time step for Reference problem - Abaqus' result.
NodeR = 6; NodeC = 104; % Reference nodes to compare PGD solution.
nrb = 1; % Number of directions on reduced basis.
% The PGD solution depends on: Space, Load, load parameter and
% displacement-velocity-acceleration for each reduced POD basis
nv = 3 + 3*nrb; % # of parameters (or variables) for the PGD solution.
%
% GEOMETRY
%
coords = load('gcoordBeam.dat'); % Nodal coordinates.
tet = load('conecBeam.dat'); % Connectivity list.
Ind = 1:size(coords,1); % List of nodes.
bcnode = Ind(coords(:,1)==min(coords(:,1))); % Boundary: Fix left side.
IndBcnode = sort([3*(bcnode-1)+1 3*(bcnode-1)+2 3*bcnode]); % D.o.f. BCs.
dof = setxor(IndBcnode,1:numel(coords))'; % D.o.f. Free nodes.
% Make use of triangulation MatLab function to obtain boundary surface.
TR = triangulation(tet,coords);
[tf] = freeBoundary(TR); % Dependent of 3D geometry of the boundary.
[tri,coors] = freeBoundary(TR); % Independent triangulation of boundary.
IndS = 1:size(coors,1); ncoors = numel(IndS);
%
% STIFNESS AND MASS MATRICES COMPUTATION
%
[r1,r2] = fem3D; r2 = Rho.*r2; % Space: Stiffness: r1, Mass: r2.
[z1,vu] = elemstiff(cooru); % Displacements.
[w1,vv] = elemstiff(coorv); % Velocities.
[s1,va] = elemstiff(coora); % Accelerations.
vu = repmat(vu,1,ncoors); % Reshape to construct source in separated form.
vv = repmat(vv,1,ncoors); 
va = repmat(va,1,ncoors);
%
% SOURCE
%
coorp = 1:size(coors,1); % We consider each possible load position as a different case.
p1 = elemstiff(coorp); % Mass matrix for load parameter: 1st vargout.
% Identifying local nodes of the loaded surface on the global connectivity.
% To obtain that: coords(IndL,:)-coors = zeros(nn2,1).
[trash,trash2,xj] = intersect(IndS,tri(:)); % TRI Local connectivity.
IndL = tf(xj); % TF Global connectivity of the loaded surface (Top).
DOFLoaded = 3*IndL; % Consider vertical load on the top of the beam.
vx = zeros(numel(coords),ncoors); 
vx(DOFLoaded,:) = eye(ncoors); % Space terms for the source.
vp = eye(ncoors); vp = p1*vp; % Load terms for the source.
%
% ACTIVATION OF LOAD PARAMETER
%
coorac = [1 2]; % Value to activate the load. Two possibilities 1-No 2-Yes.
pa1 = elemstiff(coorac); % Mass matrix for load activation: 1st vargout.
vpa = pa1*repmat([0; -Modulus],1,ncoors); % Activation terms for the source
%
% LOADING P.O.D. DATA TO CONSTRUCT REDUCED BASIS
%
WS = load('WorkSpaceBeam_REF.mat','Vreal'); 
Vreal = WS.Vreal; % Loading Displacement field of the reference solution.
%
% APPLY P.O.D. TECHNIQUE TO OBTAIN REDUCED BASIS
%
Q = Vreal(dof,:)*Vreal(dof,:)'; [A,lam] = eigs(Q,[],nrb);
%
% ALLOCATION OF MATRICES AND VECTORS FOR EACH TIME INTEGRATION STEP (1,2)
%
K1 = cell(nv,1); M1 = K1; V1 = K1; Fv1 = K1; FR1 = K1; coor1 = cell(nv,1);
K2 = cell(nv,1); M2 = K2; V2 = K2; Fv2 = K2; FR2 = K2; coor2 = cell(nv,1); 
%
% SPACE MATRICES
%
K1{1} = r1; % Stiffness matrix for SubStep 1.
M1{1} = r2; % Mass matrix for SubStep 1.
V1{1} = vx; % Space term for the source at the SubStep 1.
K2{1} = r1; M2{1} = r2; V2{1} = vx; % SubStep 2.
%
% LOAD MATRICES
%
K1{nv} = p1; % Stiffness matrix contribution of load for SubStep 1.
M1{nv} = p1; % Mass matrix for SubStep 1.
V1{nv} = vp; % Load term for the source at the SubStep 1.
coor1{nv} = coorp; % Load Discretization for SubStep 1.
K2{nv} = p1; M2{nv} = p1; V2{nv} = vp; coor2{nv} = coorp; % SubStep 2.
%
% MATRICES RELATED TO ACTIVATION PARAMETER 
%
K1{nv-1} = pa1; % Stiffness contribution of act.param. for SubStep 1.
M1{nv-1} = pa1; % Mass matrix for SubStep 1.
V1{nv-1} = vpa; % Activation parameter term for the source, SubStep 1.
coor1{nv-1} = coorac; % Activation parameter discretization for SubStep 1.
K2{nv-1} = pa1; M2{nv-1} = pa1; V2{nv-1} = vpa; coor2{nv-1} = coorac; % S2.
%
% REDUCED BASIS MATRICES
%
for i1=2:3:nv-2
    %
    % SUBSTEP 1
    %
    K1{i1} = z1; M1{i1} = z1; V1{i1} = vu; coor1{i1} = cooru; % U
    K1{i1+1} = w1; M1{i1+1} = w1; V1{i1+1} = vv; coor1{i1+1} = coorv; % V
    K1{i1+2} = s1; M1{i1+2} = s1; V1{i1+2} = va; coor1{i1+2} = coora; % A
    %
    % SUBSTEP 2
    %
    K2{i1} = z1; M2{i1} = z1; V2{i1} = vu; coor2{i1} = cooru; % U
    K2{i1+1} = w1; M2{i1+1} = w1; V2{i1+1} = vv; coor2{i1+1} = coorv; % V
    K2{i1+2} = z1; M2{i1+2} = z1; V2{i1+2} = vu; coor2{i1+2} = cooru; % U/2
end
%
% INICIALIZATING PGD SOLUTION 
%
for i1=1:nv
    Fv1{i1} = 0.0.*V1{i1}(:,1); % PGD vectors for SubStep 1.
    Fv2{i1} = 0.0.*V2{i1}(:,1); % PGD vectors for SubStep 2. 
end
%
% BOUNDARY CONDITIONS
%
Free1 = cell(nv,1); Free2 = cell(nv,1);
Free1{1} = dof; Free2{1} = dof; % Free DOF for Space.
for i1=2:nv % No BCs for rest of parameters (variables).
    Free1{i1} = 1:numel(coor1{i1});
    Free2{i1} = 1:numel(coor2{i1});
end
%
% Un, Vn, An ... IN SEPARATED FORM FOR INTEGRATION SCHEME
%
% We have 3*nen terms in the source related to Un, Vn and An in SubStep 1
% and Vn, Un+1/2 and Un for SubStep 2. Following the sort of the variables
% in the PGD solution for SubStep 1, U1_{n+1/2}(Var_1, Var_2,..., 
% Var_{nv-1}, Var_{nv}), where Var_1=Spatial Coordinates, Var_{nv-1} = 
% Activation Parameter, Var_{nv} = Loads, and Var_(2:3:nv_1) = U_n 
% (Displacement in time n), Var_(3:3:nv_1) = V_n (Velocity in time n) and
% Var_(4:3:nv_1) = A_n (Acceleration in time n)
% For the PGD solution for SubStep 2, U2_{n+1/2}(Var_1, Var_2,..., 
% Var_{nv-1}, Var_{nv}), where Var_1=Spatial Coordinates, Var_{nv-1} = 
% Activation Parameter, Var_{nv} = Loads, and Var_(2:3:nv_1) = U_n 
% (Displacement in time n), Var_(3:3:nv_1) = V_n (Velocity in time n) and 
% Var_(4:3:nv_1) = U_{n+1/2} (Displacement in time n + 1/2).
%
% IMPORTANT: To obtain U_n and V_n variables in SubStep 1 (for instance) in 
% separated form we consider that:
% U_n = 1_{space} U_n 1_{velocity} 1_{acceleration} 1_{activation} 1_{load}
% V_n = 1_{space} 1_{displac.} V_n 1_{acceleration} 1_{activation} 1_{load}
FR1{1} = zeros(size(Fv1{1},1),nv-3); FR2{1} = zeros(size(Fv2{1},1),nv-3);
for i1=2:nv
    FR1{i1} = ones(size(Fv1{i1},1),nv-3); 
    FR2{i1} = ones(size(Fv2{i1},1),nv-3); 
end
for i1=1:3 % 3 terms per # reduced basis.
    FR1{1}(dof,i1:3:end) = A; % Projection space onto Reduced basis.
    FR2{1}(dof,i1:3:end) = A;
end
for i1=2:nv-2
    FR1{i1}(:,i1-1) = coor1{i1}'; % U_n, V_n, A_n.
    FR2{i1}(:,i1-1) = coor2{i1}'; % U_n, V_n, U_{n+1/2}.
end
%
% ENRICHMENT OF THE APPROXIMATION: SUBSTEP 1
%
num_iter1 = 0; Error_iter = 1.0; iter = zeros(1); Aprt = 0;
while Error_iter>TOL && num_iter1<num_max_iter
    num_iter1 = num_iter1 + 1; R0 = cell(nv,1);
    for i1=1:nv
        R0{i1} = ones(size(Fv1{i1},1),1); % Initial guess for R, S, ...
    end;
    R0{1}(IndBcnode) = 0; % We impose initial guess for spacial coordinates
    %
    % ENRICHMENT STEP
    %
    [R,iter(num_iter1)] = enrichment_substep1(K1,M1,V1,num_iter1,Fv1,R0,...
        FR1,Free1,deltat);
    for i1=1:nv, Fv1{i1}(:,num_iter1) = R{i1}; end % R is valid, add it.
    %
    % STOPPING CRITERION
    %
    Error_iter = 1.0; 
    for i1=1:nv
        Error_iter = Error_iter.*norm(Fv1{i1}(:,num_iter1)); 
    end 
    Aprt = max(Aprt,sqrt(Error_iter));
    if num_iter1>nrb, Error_iter = sqrt(Error_iter)/Aprt; end
    fprintf(1,'SubStep 1: %dst summand in %d ',num_iter1,iter(num_iter1));
    fprintf(1,'iterations with a weight of %f\n',sqrt(Error_iter));
end
fprintf(1,'\n');
%
% ENRICHMENT OF THE APPROXIMATION: SUBSTEP 2
%
num_iter2 = 0; Error_iter = 1.0; iter = zeros(1); Aprt = 0;
while Error_iter>TOL && num_iter2<num_max_iter
    num_iter2 = num_iter2 + 1; R0 = cell(nv,1);
    for i1=1:nv
        R0{i1} = rand(size(Fv2{i1},1),1); % Initial guess for R, S, ...
    end
    R0{1}(IndBcnode) = 0; % We impose initial guess for spacial coordinates
    %
    % ENRICHMENT STEP
    %
    [R,iter(num_iter2)] = enrichment_substep2(K2,M2,V2,num_iter2,Fv2,R0,...
        FR2,Free2,deltat);
    for i1=1:nv, Fv2{i1}(:,num_iter2) = R{i1}; end % R is valid, add it.
    %
    % STOPPING CRITERION
    %
    Error_iter = 1.0;
    for i1=1:nv
        Error_iter = Error_iter.*norm(Fv2{i1}(:,num_iter2)); 
    end 
    Aprt = max(Aprt,sqrt(Error_iter));
    if num_iter2>nrb, Error_iter = sqrt(Error_iter)/Aprt; end
    fprintf(1,'SubStep 2: %dst summand in %d ',num_iter2,iter(num_iter2));
    fprintf(1,'iterations with a weight of %f\n',sqrt(Error_iter));
end
fprintf(1,'PGD Process exited normally\n\n');
save('WorkSpacePGD_Dynamic.mat');
%
% POST-PROCESSING
%
figure; % Plotting reference solution for the node NodeR
plot(0:deltatA:deltatA*(size(Vreal,2)-1),Vreal(3*(NodeR-1)+3,:),... 
    'b-','LineWidth',2.5); % Reference solution for vertical displacement
%
% ALLOCATE MEMORY FOR SUBSTEP SOLUTION VECTORS
%
Mv = cell(nv,1); % Nodal values for each parameter
% We need to compute SUBSTEP 1: MáA_{n+1/2} + KáU_{n+1/2} = F_{n+1/2}
% and A_{n+1/2} = 4*(V_{n+1/2}-V_n)/Deltat - A_n 
% and V_{n+1/2} = 4*(U_{n+1/2}-U_n)/Deltat - V_n
Disp2 = zeros(nrb,numel(coort)); % Allocate memory for U_{n+1/2}
Vel2 = zeros(nrb,numel(coort)); % Allocate memory for V_{n+1/2}
Acel2 = zeros(nrb,numel(coort)); % Allocate memory for A_{n+1/2}
% We need to compute SUBSTEP 2: MáA_{n+1} + KáU_{n+1} = F_{n+1}
% and A_{n+1} = V_n/Deltat - 4*V_{n+1/2}/Deltat + 3*V_{n+1}/Deltat 
% and V_{n+1} = U_n/Deltat - 4*U_{n+1/2}/Deltat + 3*U_{n+1}/Deltat
Disp = zeros(nrb,numel(coort)); % Allocate memory for U_{n+1}
Vel = zeros(nrb,numel(coort)); % Allocate memory for V_{n+1}
Acel = zeros(nrb,numel(coort));% Allocate memory for A_{n+1}
% We compute U_n,V_n,... onto reduced basis. We back to real Space
RealDisp2 = zeros(numel(coords),numel(coort)); 
RealDisp = zeros(numel(coords),numel(coort));
%
% INITIAL VALUES FOR DISPLACEMENT FIELD
%
Ai = pinv(A); % Pseudo-inverse P.O.D. matrix.
% 1st and 2nd displacement field for t = 0 and t = deltat from reference
% solution to start the simulation.
RealDisp(dof,1:2) = Vreal(dof,1:2).*deltat./deltatA;
% We project these first two displacements onto reduced basis.
Disp(:,1:2) = Ai*RealDisp(dof,1:2).*deltat./deltatA;
% We interpolate the reference solution for [1:2]+1/2 steps.
Disp2(:,1) = Ai*((Vreal(dof,1) + Vreal(dof,2))/2).*deltat./deltatA; 
Disp2(:,2) = Ai*((Vreal(dof,2) + Vreal(dof,3))/2).*deltat./deltatA;
%
% REAL-TIME LOOP FOR TIME INTEGRATION
%
for k2=3:numel(coort)
    %
    % APPLY SUBSTEP 1
    %
    for i1=2:3:nv-2
        Mv{i1} = evaluate_shpfunc(coor1{i1},Disp((i1+1)/3,k2-1));
        Mv{i1+1} = evaluate_shpfunc(coor1{i1+1},Vel((i1+1)/3,k2-1));
        Mv{i1+2} = evaluate_shpfunc(coor1{i1+2},Acel((i1+1)/3,k2-1));
    end
    Mv{nv} = evaluate_shpfunc(coor1{nv},NodeC); % Load case NodeC.
    if coort(k2)-deltat/2<0.25
        Mv{nv-1} = [0; 1]; % Value of the shape function for act. parameter
    elseif coort(k2)-deltat/2>=0.5 % Time when the load vanishes
        Mv{nv-1} = [1; 0]; % Value of the shape function for act. parameter
    else % Ramp case 
        Mv{nv-1}(2) = (0.5-(coort(k2)-deltat/2))/0.25;
        Mv{nv-1}(1) = 1-Mv{nv-1}(2); % Value of the shape function.
    end
    %
    % COMPUTE PGD SUBSTEP 1 SOLUTION
    %
    for k1=1:num_iter1
        value1 = Fv1{1}(dof,k1);
        for j1=2:nv, value1 = value1.*(Mv{j1}'*Fv1{j1}(:,k1)); end
        RealDisp2(dof,k2) = RealDisp2(dof,k2) + value1;
    end
    %
    % TIME SCHEME SUBSTEP 1
    %
    % MáA_{n+1/2} + KáU_{n+1/2} = F_{n+1/2}
    % and A_{n+1/2} = 4*(V_{n+1/2}-V_n)/Deltat - A_n 
    % and V_{n+1/2} = 4*(U_{n+1/2}-U_n)/Deltat - V_n
    Disp2(:,k2) = Ai*RealDisp2(dof,k2); % Project onto reduced basis
    Vel2(:,k2) = 4*(Disp2(:,k2)-Disp(:,k2-1))/deltat - Vel(:,k2-1);
    Acel2(:,k2) = 4*(Vel2(:,k2)-Vel(:,k2-1))/deltat - Acel(:,k2-1);
    %
    % APPLY SUBSTEP 2
    %   
    for i1=2:3:nv-2
        Mv{i1+2} = evaluate_shpfunc(coor2{i1+2},Disp2((i1+1)/3,k2));
    end
    Mv{nv} = evaluate_shpfunc(coor1{nv},NodeC); % Load case NodeC.
    if coort(k2)<0.25
        Mv{nv-1} = [0; 1]; % Value of the shape function for act. parameter
    elseif coort(k2)>=0.5 % Time when the load vanishes
        Mv{nv-1} = [1; 0]; % Value of the shape function for act. parameter
    else % Ramp case 
        Mv{nv-1}(2) = (0.5-coort(k2))/0.25;
        Mv{nv-1}(1) = 1-Mv{nv-1}(2); % Value of the shape function.
    end
    %
    % COMPUTE PGD SUBSTEP 2 SOLUTION
    %
    for k1=1:num_iter2
        value2 = Fv2{1}(dof,k1);
        for j1=2:nv, value2 = value2.*(Mv{j1}'*Fv2{j1}(:,k1)); end
        RealDisp(dof,k2) = RealDisp(dof,k2) + value2;
    end
    %
    % TIME SCHEME SUBSTEP 2
    %
    % MáA_{n+1} + KáU_{n+1} = F_{n+1}
    % and A_{n+1} = V_n/Deltat - 4*V_{n+1/2}/Deltat + 3*V_{n+1}/Deltat 
    % and V_{n+1} = U_n/Deltat - 4*U_{n+1/2}/Deltat + 3*U_{n+1}/Deltat
    Disp(:,k2) = Ai*RealDisp(dof,k2); % Project onto reduced basis
    Vel(:,k2) = Disp(:,k2-1)/deltat - 4*Disp2(:,k2)/deltat + ...
        3*Disp(:,k2)/deltat;
    Acel(:,k2) = Vel(:,k2-1)/deltat - 4*Vel2(:,k2)/deltat + ...
        3*Vel(:,k2)/deltat;
end
%
% PLOT PGD FINAL SOLUTION AND COMPARE WITH THE REFERENCE SOLUTION
%
hold on; plot(coort,RealDisp(3*(NodeR-1)+3,:),'m--','LineWidth',2.5);
save('WorkSpacePGD_Dynamic.mat');
fprintf(1,'\n#####ÊEnd of simulation #####\n\n');