% Calculates the inter-storey displacement D, lateral stiffness ST,
% integration constants C1 & C2, maximum deflection ymax, column
% deflections yi
%   P = (n+1) vector of total axial load
%   Ec = (n+1) vector of column moduli
%   Ic = (n+1) vector of column inertia
%   Lc = (n+1) vector of column lengths
%   ru,rl = (n+1) vectors of end fixity
%   beta = (n+1) vector of stiffness coefficients
%   chi = (n+1) vector of coefficients relating to out-of-straightness
%   d0 = scalar or (n+1) vector of out-of-straightness
%   D0 = scalar or (n+1) vector of out-of-plumbness
%   Kb = scalar, total semi-bracing stiffness
%   Q = scalar, lateral force
%   worst_case = boolean - if true (1), sets chi*d0 to absolute values
%   n_seg = scalar, number of segments to discretize when computing deflections
function [D,ST,C1,C2,yi,ymax] = Get_Deflection(P,Ec,Ic,Lc,ru,rl,beta,chi,d0,D0,worst_case,Q,Kb,n_seg)
    % Verify input
    n = length(Ec)-1;
    if (length(Ic) ~= n+1 || length(Lc)~=n+1 || length(beta)~=n+1 ||...
            length(P)~=n+1)
        D = -100;
        ST = -100;
        C1 = -100;
        C2 = -100;
        yi = -100;
        ymax = -100;
        return;
    end
    if (length(d0) == 1)
        d0 = ones(1,n+1)*d0;
    end
    if (length(D0) == 1)
        D0 = ones(1,n+1)*D0;
    end 
    
    Si = zeros(1,n+1);
    
    ND = 0;
    Nd = 0;
    for i=1:n+1
        
        % Sum D0
        abs_term = D0(i);
        if (worst_case == 1)
            abs_term = abs(abs_term);
        end
        ND = ND + P(i)/Lc(i)*abs_term;
        
        % Sum d0
        abs_term = d0(i)*chi(i);
        if (worst_case == 1)
            abs_term = abs(abs_term);
        end
        Nd = Nd + P(i)/Lc(i)*abs_term;
        
        % Tangent stiffness
        Si(i)= 12*Ec(i)*Ic(i)/Lc(i)^3*beta(i);
    end
    ST = sum(Si);
    if (ST+Kb > 0)
        D = (Q+ND+Nd)/(ST+Kb);
    else
        D = 1/0;
    end
    
    % Calculate maximum deflections
    % Warning - do NOT use this with worst_case = 1!!
    %Rl = 3*Ec.*Ic./Lc.*rl./(1-rl);
    phi = Lc.*(P./(Ec.*Ic)).^0.5;
    Y = zeros(1,n+1);
    yi = zeros(1,n+1);
    %DC = zeros(1,n+1);
    C1 = zeros(1,n+1);
    C2 = zeros(1,n+1);
    tlRl = zeros(1,n+1);
    %e11 = zeros(1,n+1);
    %e12 = zeros(1,n+1);
    %e21 = zeros(1,n+1);
    %e22 = zeros(1,n+1);
    for i=1:n+1
        SIN = sin(phi(i));
        COS = cos(phi(i));
        %DC(i)= phi(i)^3*SIN*(1-ru(i))*(1-rl(i)) + 9*rl(i)*ru(i)*(2-2*COS-phi(i)*SIN) + 3*phi(i)*(rl(i)+ru(i)-2*rl(i)*ru(i))*(SIN-phi(i)*COS);
        %e11(i)=rl(i)*phi(i)*( 9*ru(i)*phi(i)*(1+COS) - 18*ru(i)*SIN - 3*(1-ru(i))*phi(i)^2*SIN );
        %e21(i)=phi(i)*( 9*ru(i)*rl(i)*phi(i)*SIN - 18*ru(i)*rl(i)*(1-COS) - 3*phi(i)^2*( ru(i)*(1-rl(i)) - rl(i)*(1-ru(i))*COS ) );
        %e12(i)=9*ru(i)*rl(i)*(1-COS) + 3*rl(i)*(1-ru(i))*phi(i)*SIN;
        %e22(i)=9*ru(i)*rl(i)*SIN + 3*phi(i)*(ru(i)*(1-rl(i)) + rl(i)*(1-ru(i))*COS);
        a1 = 3*rl(i)+3*ru(i)-6*rl(i)*ru(i);
        a2 = 9*rl(i)*ru(i) - phi(i)^2*(1-ru(i))*(1-rl(i));
        a3 = 18*rl(i)*ru(i)+a1*phi(i)^2;
        C_denom = 18*rl(i)*ru(i) - a3*cos(phi(i))+(a1-a2)*phi(i)*sin(phi(i));
        C1(i) = 3*rl(i)/C_denom*(...
            d0(i)*pi/(pi^2-phi(i)^2)*(3*phi(i)^2*ru(i)*(1+cos(phi(i))) - 6*phi(i)*ru(i)*sin(phi(i)) - phi(i)^3*sin(phi(i))*(1-ru(i))) -...
            D*( 3*ru(i)*(1-cos(phi(i))) + phi(i)*sin(phi(i))*(1-ru(i)) ) );
        C2(i) = 3/C_denom*(...
            d0(i)*pi/(pi^2-phi(i)^2)*(3*phi(i)^2*rl(i)*ru(i)*sin(phi(i)) - 6*phi(i)*rl(i)*ru(i)*(1-cos(phi(i))) - phi(i)^3*(ru(i)*(1-rl(i)) - rl(i)*cos(phi(i))*(1-ru(i)))) + ...
            D*(phi(i)*(ru(i)*(1-rl(i))+rl(i)*cos(phi(i))*(1-ru(i)))+3*rl(i)*ru(i)*sin(phi(i)))...
            );
        Y(i) = Si(i)*D - P(i)/Lc(i)*D0(i) - P(i)/Lc(i)*d0(i)*chi(i);
        %tl(i) = C2(i)*phi(i)/Lc(i)*1 - phi(i)^2*d0(i)*pi/Lc(i)*1/(phi(i)^2-pi^2) - D0(i)/Lc(i) - Y(i)/P(i);
        tlRl(i) = 3*Ec(i)*Ic(i)/Lc(i)*rl(i)*(D*phi(i)^2*(pi^2-phi(i)^2)*((1-ru(i))*phi(i)*SIN + 3*ru(i)*(1-COS)) + d0(i)*phi(i)^3*pi*(phi(i)^2*SIN*(1-ru(i))+3*ru(i)*(2*SIN-phi(i)*(1+COS))))/( Lc(i)*(pi^2-phi(i)^2)*C_denom );
        
        % Find greatest deflection in column
        for x=0:Lc(i)/n_seg:Lc(i)
            yx = C1(i)*cos(phi(i)*x/Lc(i)) + C2(i)*sin(phi(i)*x/Lc(i)) + tlRl(i)/P(i) - phi(i)^2*d0(i)*sin(pi*x/Lc(i))/(phi(i)^2-pi^2) - D0(i)*x/Lc(i) - Y(i)*x/P(i); 
            y1 = d0(i)*sin(pi*x/Lc(i));
            y2 = D0(i)*x/Lc(i);
            if (abs(yx+y1+y2) > abs(yi(i)))
                yi(i) = yx+y1+y2;
            end
            if (Lc(i) == x && P(i) ~= 0 && D ~= 0 && D~=Inf)
                if abs(yx - D)<1e-10
                    %fprintf('\nGOOD %.4f , %.4e',d0(i),yx-D)
                else
                    fprintf('\nWarning - Column upper end deflection does not match y(L)! %.4f , %.4e, %.0f',d0(i),yx-D, i)
                end
            end
        end
        index = find(abs(yi)==max(abs(yi)));
        ymax = yi(index(1));
    end
    
end