% This program corresponds to Appendix 7.7.2 of the thesis.
% The effective rotational stiffness contribution of a three-segment member
% towards the columns at its ends is derived. The member has an axial force.
clc, clear, close all

% Governing differential equations
syms Rl tl Yj N w1 w2 w3 p1 p2 p3 yi yj x L
syms C1 C2 C3 C4 C5 C6 
y1(x) = yi - Rl*tl/N - Yj*x/N + C1*cos(w1*p1/L*x) + C2*sin(w1*p1/L*x);
y2(x) = yi - Rl*tl/N - Yj*x/N + C3*cos(w2*p2/L*x) + C4*sin(w2*p2/L*x);
y3(x) = yi - Rl*tl/N - Yj*x/N + C5*cos(w3*p3/L*x) + C6*sin(w3*p3/L*x);
psi1(x) = -Yj/N + p1/(w1*L) * ( C2*cos(w1*p1/L*x) - C1*sin(w1*p1/L*x) );
psi2(x) = -Yj/N + p2/(w2*L) * ( C4*cos(w2*p2/L*x) - C3*sin(w2*p2/L*x) );
psi3(x) = -Yj/N + p3/(w3*L) * ( C6*cos(w3*p3/L*x) - C5*sin(w3*p3/L*x) );

% Substitute boundary conditions
syms L1 L2 L3 tu Ru Tl Tu
eqn1 = subs(y1,x,0) == yi;
eqn2 = subs(y3,x,L) == yj;
eqn3 = subs(psi1,x,0) == Tl-tl;
eqn4 = subs(psi3,x,L) == Tu-tu;
eqn5 = tu*Ru + tl*Rl + Yj*L == -N*(yj-yi);
eqn6 = L1+L2+L3 == L;
eqn7 = subs(y1,x,L1) == subs(y2,x,L1);
eqn8 = subs(y2,x,L-L3) == subs(y3,x,L-L3); % Changed to L-L3 to make the solution more symmetricly apparently
eqn9 = subs(psi1,x,L1) == subs(psi2,x,L1);
eqn10 = subs(psi2,x,L-L3) == subs(psi3,x,L-L3); % Changed to L-L3 to make the solution more symmetricly apparently

% Solve the system of equations
[A,B] = equationsToMatrix([eqn1,eqn2,eqn3,eqn4,eqn5,eqn6,eqn7,eqn8,eqn9,eqn10],[Yj,tu,tl,C1,C2,C3,C4,C5,C6,L2]);
X = linsolve(A,B)
ti_f = simplify(X(3));
tj_f = simplify(X(2));

% Invert the rotational stiffness matrix
[rotate_matrix,b] = equationsToMatrix([X(3);X(2)]==[tl; tu],[Tl;Tu;yi;yj]);
R_matrix = simplify([Rl 0; 0 Ru]*rotate_matrix)
syms EI0 ri rj taul tauu 
R_matrix = simplify(subs(R_matrix,[Rl Ru],3*EI0/L*[ri/(1-ri)/taul rj/(1-rj)/tauu]))

% Test for numerical symmetry and equality
syms kAG1 kAG2 kAG3 EI1 EI2 EI3 kAG0 u1 u2 u3 m1 m2 m3
R_matrix_num = R_matrix;
R_matrix_num = subs(R_matrix_num,[p1 p2 p3],[L*(N/EI1)^0.5 L*(N/EI2)^0.5 L*(N/EI3)^0.5]);
R_matrix_num = subs(R_matrix_num,[EI1 EI2 EI3],EI0*[m1 m2 m3]);
R_matrix_num = subs(R_matrix_num,[w1 w2 w3],[(1+N/kAG1)^0.5  (1+N/kAG2)^0.5 (1+N/kAG3)^0.5]);
R_matrix_num = subs(R_matrix_num,[kAG1 kAG2 kAG3], kAG0*[u1 u2 u3]);
R_matrix_num = subs(R_matrix_num, [ri rj], [1/(1+3*EI0/(L*Rl*taul)) 1/(1+3*EI0/(L*Ru*tauu))]);
R_matrix_num = subs(R_matrix_num, [taul, tauu], [1/m1*(L1/L)^3 + 1/m2*(((L1+L2)/L)^3 - (L1/L)^3) + 1/m3*(1-((L1+L2)/L)^3), 1/m3*(L3/L)^3 + 1/m2*(((L3+L2)/L)^3 - (L3/L)^3) + 1/m1*(1-((L3+L2)/L)^3)]);
R_matrix_num = subs(R_matrix_num,L,L1+L2+L3);
vec_vars = [N L1 L2 L3 EI0 m1 m2 m3 kAG0 u1 u2 u3 Rl Ru];
vec_nums_1 = [1.2e5 1.486 2.184 0.987 2e11*1.7e-3 0.85 0.81 0.90 0.44*76e9*1e-3 0.74 0.95 0.37 4.02e7 2.86e7];
vec_nums_2 = [1.2e5 0.987 2.184 1.486 2e11*1.7e-3 0.90 0.81 0.85 0.44*76e9*1e-3 0.37 0.95 0.74 2.86e7 4.02e7];
R_matrix_num_1 = double(subs(R_matrix_num,vec_vars,vec_nums_1)) % these should be symmetric
R_matrix_num_2 = double(subs(R_matrix_num,vec_vars,vec_nums_2)) % these should be symmetric
% If  R12 = R21, R13 = -R14, R23 = -R24, R11 sym R22, R13 sym R23 then OK to proceed
% Also verified that denominator is the same for all terms

% Verification of the hand-simplified solution (numerical results only; solution is too long)
vul = Tu/Tl;
wul = (yj-yi)/(L*Tl);
Rij_sol = R_matrix(1,1) + R_matrix(1,2)*vul - R_matrix(1,3)*wul*L;
Rij_sol = subs(Rij_sol,[p1 p2 p3],[L*(N/EI1)^0.5 L*(N/EI2)^0.5 L*(N/EI3)^0.5]);
Rij_sol = subs(Rij_sol,[EI1 EI2 EI3],EI0*[m1 m2 m3]);
Rij_sol = subs(Rij_sol,[w1 w2 w3],[(1+N/kAG1)^0.5  (1+N/kAG2)^0.5 (1+N/kAG3)^0.5]);
Rij_sol = subs(Rij_sol,[kAG1 kAG2 kAG3], kAG0*[u1 u2 u3]);
Rij_sol = subs(Rij_sol, [ri rj], [1/(1+3*EI0/(L*Rl*taul)) 1/(1+3*EI0/(L*Ru*tauu))]);
Rij_sol = subs(Rij_sol, [taul, tauu], [1/m1*(L1/L)^3 + 1/m2*(((L1+L2)/L)^3 - (L1/L)^3) + 1/m3*(1-((L1+L2)/L)^3), 1/m3*(L3/L)^3 + 1/m2*(((L3+L2)/L)^3 - (L3/L)^3) + 1/m1*(1-((L3+L2)/L)^3)]);
Rij_sol = subs(Rij_sol,L,L1+L2+L3);
Rij_sol_num = subs(Rij_sol,vec_vars,vec_nums_1)
Rij_sol_num = double(subs(Rij_sol_num,[yi,yj,Tu,Tl],[0.05258,0.10862,0.006812,0.038496]))

L = L1+L2+L3;
kAG1 = kAG0*u1;
kAG2 = kAG0*u2;
kAG3 = kAG0*u3;
w1 = (1+N/kAG1)^0.5;
w2 = (1+N/kAG2)^0.5;
w3 = (1+N/kAG3)^0.5;
taul = 1/m1*(L1/L)^3 + 1/m2*(((L1+L2)/L)^3 - (L1/L)^3) + 1/m3*(1-((L1+L2)/L)^3);
tauu = 1/m3*(L3/L)^3 + 1/m2*(((L3+L2)/L)^3 - (L3/L)^3) + 1/m1*(1-((L3+L2)/L)^3);
ri = 1/(1+3*EI0/(L*Rl*taul));
rj = 1/(1+3*EI0/(L*Ru*tauu));
p0 = L*(N/EI0)^0.5;
p1 = p0/m1^0.5;
p2 = p0/m2^0.5;
p3 = p0/m3^0.5;
a1 = f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,0,2,0)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,1,0)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,0,1,1)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,0,1);
a2 = f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,2,1,1)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,2,1)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,1,2)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,2,0,2);
a3 = f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,2,1,0)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,2,0)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,2,0,1)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,1,1);
a4 = f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,0,2)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,0,1,2)...
    +f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,0,2,1)...
    -f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,1,1,1);
a5 = a3+a4 + 2*p1*p2*p3*w1*w2*w3;
a0 = 3*p0^2*rj*(a4-a1) - p0^4*tauu*(1-rj)*a1;
av = p1*p2*p3*w1*w2*w3 + a1;
aw = 3*p0^2*rj*(a4+p1*p2*p3*w1*w2*w3) - p0^4*tauu*(1-rj)*a1;
NUM = a0 + 3*rj*p0^2*av*vul - aw*wul;
DEN = 9*rj*ri*(a5-a2)...
    + 3*p0^2*(  ri*(1-rj)*tauu*(a3-a1) + rj*(1-ri)*taul*(a4-a1)  )...
    -p0^4*taul*tauu*(1-ri)*(1-rj)*a1;
Rij_try = 3*EI0/L*ri*NUM/DEN;
Rij_try_num = subs(Rij_try,vec_vars,vec_nums_1)
Rij_try_num = double(subs(Rij_try_num,['L',yi,yj,Tu,Tl],[1.486+2.184+0.987,0.05258,0.10862,0.006812,0.038496]))
zero_num = Rij_try_num-Rij_sol_num % should be zero

% Converts p1^a * p2^b * p3^c into the appropriate terms
% Make sure the input p1,p2,p3,w1,w2,w3,L1,L2,L3,L are symbolic
function [terms] = f(p1,p2,p3,w1,w2,w3,L1,L2,L3,L,a,b,c) 
    terms = p1^a*p2^b*p3^c;
    terms = terms*w1^(2-a)*w2^(2-b)*w3^(2-c);
    C1 = cos(w1*p1*L1/L); S1 = sin(w1*p1*L1/L);
    C2 = cos(w2*p2*L2/L); S2 = sin(w2*p2*L2/L);
    C3 = cos(w3*p3*L3/L); S3 = sin(w3*p3*L3/L);
    terms = terms*C1^mod(a,2)*C2^mod(b,2)*C3^mod(c,2);
    terms = terms*S1^mod(a+1,2)*S2^mod(b+1,2)*S3^mod(c+1,2);
end

