% This program corresponds to Appendix 7.3 of the thesis.
% It numerically solves the stiffness matrix for an n-storey column, given:
%   H =  m×1 vector of storey lengths (level 1 up)
%   R =  (m+1)×1 vector of rotational rigidities at each level (ground up)
%   P =  m×1 vector of applied gravity loads at each storey level (level 1 up) -- +ve comp, -ve tens
%   EI = m×1 vector of column stiffness EI at each storey level (level 1 up)
%   bool_shear = 1 to toggle shear deformations
%   kAG = m×1 vector of transverse shear stiffness at each store level (level 1 up)
%   shear_assumption = 0 for Engesser, 1 for Haringx

% Note: Assume column splices are continuous
function [K] = Subs_K_matrix_m_storeys(H, R, P, EI,...
    bool_shear,kAG,shear_assumption) % shear
syms x

% Calculate axial forces at each level
m = length(H); % Number of storeys
N = zeros(1,m);
N(m) = P(m);
for j = m-1:-1:1
    N(j) = N(j+1) + P(j);
end
omega = ones(1,m);
wphi = zeros(1,m);
for j=1:m
    if bool_shear == 1
        if shear_assumption == 0
            omega(j) = sqrt(1+N(j)/kAG(j));
        else %%shear_asumption == 1
            omega(j) = 1/sqrt(1-N(j)/kAG(j));
        end
    else
        omega(j) = 1;
    end
    wphi(j) = omega(j)* sum(H)*(abs(N(j))/EI(j))^0.5;
end

% Calculate the height at each storey level
xj = zeros(1,m);
for j=1:m
    xj(j) = 0;
    for jj = 1:j
        xj(j) = xj(j) + H(jj); 
    end
end

% Define variables
syms t0                 % Rotation of ground connection
t = sym('t',[1 m]);     % Rotation for storeys 1 to m
D = sym('D',[1 m]);     % Deflection for storeys 1 to m
Q = sym('Q',[1 m]);     % Arbitrary lateral load for storeys 1 to m
R0 = R(1);              % Rotational stiffness at ground connection
R = R(2:length(R));     % Rotational stiffness for storeys 1 to m
C = sym('C',[1 2*m]);   % Integration constants (2 per storey)

% General solutions to the system of DE's
for j = 1:m
    sumtR = t0*R0;  % Sum of tj*Rj to j-1
    sumPD = 0;      % Sum of P*Delta to j-1
    sumQx_bot = 0;  % Sum of Qj*xj from storey 1 to storey j-1
    sumQx_top = 0;  % Sum of Qj*x  from storey i to storey m
    for jj = 1:j-1
        sumtR = sumtR + t(jj)*R(jj);
        sumDj = 0;  % Sum of deflections at storey level jj
        for jjj=1:jj
            sumDj = sumDj + D(jjj); 
        end
        sumPD = sumPD + P(jj)*sumDj;
        sumQx_bot = sumQx_bot + Q(jj)*xj(jj);
    end
    for jj = j:m
        sumQx_top = sumQx_top + Q(jj)*x;
    end
    
    if N(j) == 0  % First order has different y(x) function (absence of N)
        y(j) = 1/EI(j)*(  (sumtR-sumPD-sumQx_bot)*x^2/2 - sumQx_top*x^2/6 ... % note x^2!! sumQx contains x already
                             + C(2*j-1)*x + C(2*j)  );
    elseif N(j) > 0 % Second order (N present and compressive)
        y(j) = (sumtR - sumPD)/N(j) +...
            C(2*j-1)*cos(wphi(j)/sum(H)*x) + C(2*j)*sin(wphi(j)/sum(H)*x) -...
            (sumQx_bot+sumQx_top)/N(j);
    else % N(j) < 0, second order (N present and tensile)
        
        % Note - tension and shear not implemented yet although it's likely it'll just be phi = wphi
        if bool_shear == 1
            disp('WARNING - TENSION AND SHEAR COMBINED FORMULATION NOT IMPLEMENTED')
        end
        y(j) = (sumtR - sumPD)/N(j) +...
            C(2*j-1)*exp(wphi(j)/sum(H)*x) + C(2*j)*exp(-wphi(j)/sum(H)*x) -...
            (sumQx_bot+sumQx_top)/N(j);
    end
    
    if bool_shear == 1
        yp(j) = 1/omega(j)^2 *(wphi(j)/sum(H))* (...
            C(2*j)*cos(wphi(j)/sum(H)*x) - C(2*j-1)*sin(wphi(j)/sum(H)*x)) ...
            - simplify((sumQx_top/x))/N(j); % Note - this is actually psi, but using the symbol yp out of convenience
    else
        yp(j) = diff(y(j),x);
    end
end

% Compatibility equations
CEy = sym('CEy',[1 m-1]);   % Compatibility for continuity of y
CEyp= sym('CEyp',[1 m-1]);  % Compatibility for continuity of y'
for j=1:m-1
    CEy(j) = subs(y(j),x,xj(j)) == subs(y(j+1),x,xj(j));
    CEyp(j)= subs(yp(j),x,xj(j)) == subs(yp(j+1),x,xj(j));
end

% Boundary Conditions
BCy = sym('BCy',[1 m+1]);
BCyp = sym('BCyp',[1 m+1]);
BCy(1) = subs(y(1),x,0) == 0;
BCyp(1)= subs(yp(1),x,0) == t0;
for j=1:m
    sumDj = 0;
    for jj=1:j
        sumDj = sumDj + D(jj);
    end
    BCy(j+1) = subs(y(j),x,xj(j)) == sumDj;
    BCyp(j+1)= subs(yp(j),x,xj(j)) == t(j);
end

% Equilibrium Equation
sumPD = 0;
for j=1:m
    sumDj = 0;
    for jj=1:j
        sumDj = sumDj + D(jj);
    end
    sumPD = sumPD + P(j)*sumDj;
end
EE = t0*R0 + sum(t.*R) == sumPD + sum(Q.*xj);

% Solve the system by isolating unknowns
try
    [A,B] = equationsToMatrix(vpa([CEy CEyp BCy BCyp EE]),[Q C t0 t]);
catch
    % Error - could not formulate the matrix
    disp('Error incoming - displaying input arguments')
    H
    R
    P
    EI
    bool_shear
    kAG
    shear_assumption
    K = Inf;
    return;
end
X = linsolve(A,B);

% Detect inconsistent or rank deficient systems
if sum(isinf(X)) == length(X)
   fprintf('\nWarning - inconsistent or rank deficient system')
   R_orig(1) = R0;
   R_orig(2:m+1) = R;
   if R_orig(1) > 1e12
    R_orig(1) = R_orig(1)^0.7; % Bug fix - inconsistent or rank deficient system occurs when R0 is too large it seems
   end
   K = Subs_K_matrix_m_storeys(double(H), double(R_orig), double(P), double(EI), bool_shear, double(kAG), shear_assumption);
   return;
end

% Represent as Q = KD
[K,b] = equationsToMatrix(transpose(Q)==X(1:length(Q)),transpose(D));
if isequaln(b,transpose(-Q)) % If MATLAB flips the Q vector and returns -K, flip it back
    K = -K;
end
K = double(K);

end
