% partitioned solution of  three bar-element problem
%
%       (1)     (2)      (3)
%
%   |0--------0-------0--------0--------0
%   
%    0        1       2        3        4
%
%   |0--------0-------0   0--------0--------0
%   
%    0        1       2   3        4        5

ke =[1  -1   0;  -1  2  -1;   0   -1  1];
z3 = zeros(3);
e1=1; e2 = 10; 

% substructural stiffness  K
Ksub =[
e1*ke   z3;
z3    e2*ke];
% bounday condition at the left end
K=Ksub;
K(1,:)=[]; K(:,1)=[];


% the bounday Boolean matrix B
B =[
0  1  0  0  0;
0  0  1  0  0]';

% the frame matrix L 
Lf =[1; 1];

% global assembly matrix  Lg
Lg  =[
eye(3)      zeros(3,2);
zeros(3,2)  eye(3)];

% Global stiffness matrix Kg 
Kg = Lg' * Ksub * Lg;
% apply BCs
Kg(1,:)=[]; Kg(:,1)=[];
Lg(1,:)=[]; Lg(:,1)=[];

% applied load in global nodes 
fg =[0  0  0  1]';
% obtain the solution from assembled eq  Kg*ug = fg
[KgL, KgU]= lu(Kg);
ug=KgU\(KgL\fg);

% plot the solution
usol =[0;ug];
node =0:4; node = node';
figure(1);
plot(node, usol, '-');
xlabel('nodes');
ylabel('displacement');
%legend([ 'middle spring (km) =', num2str(km),   '  beta*L =  ', num2str(2*betaL_accurate)]);
title('Solution of Three bar problem with tip load by assembled equation');
hold;

% applied load in partitioned nodes
fsub  = pinv(Lg') * fg;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve the problem by a naive partitioned solution procedure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Kpinv = pinv(K);
u_initial = Kpinv* fsub;

Fb = B'*Kpinv*B; 
Kb = pinv(Fb);
KbbS = (Lf'*Kb*Lf);
[LKbbS, UKbbS] = lu(KbbS);

lambda_initial = Kb*(B'*u_initial);


uf = UKbbS\(LKbbS\(Lf'*lambda_initial));
lambda  = lambda_initial-Kb*(Lf*uf);
u_naive = u_initial - Kpinv*(B*lambda);
% compute the global displacement
ug_naive  = pinv(Lg)*u_naive;


% plot the solution
usol =[0;ug_naive];
node =0:4; node = node';
figure(2);
plot(node, usol, '-');
xlabel('nodes');
ylabel('displacement');
%legend([ 'middle spring (km) =', num2str(km),   '  beta*L =  ', num2str(2*betaL_accurate)]);
title('Solution of three bar problem with tip load by naive partitioned equation');
 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve the problem by the correct basis partitioned solution procedure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%the rigid body  modes
R =(1/sqrt(3))*[
0 0 1 1 1]';

Rb = B'*R;

Kpinv = pinv(K);
d_initial = Kpinv* fsub;
g_lambda = B'*d_initial;
g_alpha = R'*fsub;

Fb = B'*Kpinv*B; 
Kb = pinv(Fb);

Pk = Kb - Kb*Lf*inv(Lf'*Kb*Lf)*Lf'*Kb;
A_alpha = Rb'*Pk*Rb;

alpha = pinv(A_alpha)*(g_alpha - Rb'*Pk*g_lambda);

lambda  =  Pk*(g_lambda + Rb*alpha);

% compute the deformation vector 
d_part = d_initial - Kpinv*(B*lambda);

% the rigid body displacement
u_part = d_part + R*alpha;

% compute the global displacement
ug_part  = pinv(Lg)*u_part;


% plot the solution
usol =[0;ug_part];
node =0:4; node = node';
figure(3);
plot(node, usol, '-');
xlabel('nodes');
ylabel('displacement');
%legend([ 'middle spring (km) =', num2str(km),   '  beta*L =  ', num2str(2*betaL_accurate)]);
title('Solution of three bar problem with tip load by correct partitioned equation');



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve the problem by the projected lambda-solution procedure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%the rigid body  modes
R =(1/sqrt(3))*[
0 0 1 1 1]';

Rb = B'*R;

Kpinv = pinv(K);
d_initial = Kpinv* fsub;
g_lambda = B'*d_initial;
g_alpha = R'*fsub;

Fb = B'*Kpinv*B; 

Pl = eye(size(B,2)) - Lf*inv(Lf'*Lf)*Lf';

C = Rb'*Pl*Rb;
Cinv = inv(C);
lambda_initial = Pl*Rb*Cinv*R'*fsub;
Proj  = Pl - Pl*Rb*Cinv*Rb'*Pl;

mu =pinv(Proj*Fb*Proj)*Proj*g_lambda;
lambda = lambda_initial + Proj*mu;

alpha = -Cinv*Rb'*Pl *(g_lambda - Fb*lambda);


% compute the deformation vector 
d_proj = d_initial - Kpinv*(B*lambda);

% the rigid body displacement
u_proj = d_proj + R*alpha;

% compute the global displacement
ug_proj  = pinv(Lg)*u_proj;


% plot the solution
usol =[0;ug_proj];
node =0:4; node = node';
figure(4);
plot(node, usol, '-');
xlabel('nodes');
ylabel('displacement');
%legend([ 'middle spring (km) =', num2str(km),   '  beta*L =  ', num2str(2*betaL_accurate)]);
title('Solution of three bar problem with tip load by projected procedure');