function Aeroelastic_SA_AL

% This code formulates the Structural and Aerodynamic Model for a wing
% represented using an Euler-Bernoulli beam of n elements using the Finite
% Element Method. It develops an aeroelastic model which is used for 
% flutter analysis to determine the flutter speed and flutter frequency. 

% The electric propulsion system is represented as nodal lumped masses at
% chosen nodes, with a thrust applied at each node.


%%% EXPECTED OUTPUTS
% Vf = flutter speed
% wf = flutter frequency

%%% REQUIRED INPUTS
% The code requires specific data to be inputted which relates to the model
% The inputs will be split into sections - 1) Structural Model Inputs
% 2) Aerodynamic Model Inputs 3) Electric Propulsion Inputs

n = 32;                         % number of elements
% selected based on results of mesh convergence study

% 1) STRUCTURAL MODEL INPUTS 
L=7.5;                              % Length of beam [m]
Lel=L/n;                            % Length of each element 
h = 0.24;                           % Height of beam [m]
b = 2;                              % Width of beam [m]

rho_beam = 2770;              	% Density [kg/m^3]
E = 73.1*10^9;                  % Young's modulus [Pa]
G = 28*10^9;                    % Shear Modulus

aa = b/2;
bb = h/2;
J = aa*(bb^3)*((16/3)-3.36*(bb/aa)*(1-(bb^4)/(12*aa^4))); % Torsion constant

%Damping Parameters
alpha = 1.658525;
beta = 0.000229;

% Calculations
I_x = h^3*b/12;                 %Area moment of inertia - X direction
I_y = b^3*h/12;                 %Area moment of inertia - Y direction

m0=b*h*rho_beam;             	% Mass per unit length
Xt = (m0*J)/(b*h);


% 2) AERODYNAMIC MODEL INPUTS
c = b;                  % chord
dz = Lel;                 % width of strip
a = 2*vpa(pi);                  % wing lift curve slope
e = 0.25;                  % wing eccentricity
M_thetadot = -0.3;         % Pitch damping term
rho = 0.412;            % air density at 10000m
%rho = 1.225;           % air density at sea level

% Range of Airspeeds
V_lb = 0;
V_ub = 500;
V_int = 5;

% Calculations 
syms V
q = 0.5*rho*V^2;
cza = c*dz*a;
ecza = e*c^2*dz*a;
czM = c^3*dz*M_thetadot;


%%% 3) ELECTRIC PROPULSION INPUTS

% Propellor properties - lumped mass
massnodes = [9 17 25 33];                 % enter nodes where lumped masses exist

m = 500;            % mass of propellor [kg]
ypos = 0.12;
xpos = 1;

% Propellor properties - thrust
Ft = 1000;


Ix = m*ypos^2;             % moment of inertia - x 
Iy = m*xpos^2;             % moment of inertia - y
Jz = (ypos^2+xpos^2)*m;             % polar moment of inertia


%%% STRUCTURAL MODEL
% Declare stiffness and mass matrix variables
M = zeros(10);
K = zeros(10);

% bernoulli beam - x direction
% Thrust
M_x = [(13/35)*m0*Lel, (11/210)*m0*Lel^2, (9/70)*m0*Lel, (-13/420)*m0*Lel^2; 
      (11/210)*m0*Lel^2, (1/105)*m0*Lel^3, (13/420)*m0*Lel^2, (-1/140)*m0*Lel^3; 
      (9/70)*m0*Lel, (13/420)*m0*Lel^2, (13/35)*m0*Lel, (-11/210)*m0*Lel^2; 
      (-13/420)*m0*Lel^2, (-1/140)*m0*Lel^3, (-11/210)*m0*Lel^2, (1/105)*m0*Lel^3];

K_x = [12*(E*I_x/Lel^3), 6*(E*I_x/Lel^2), -12*(E*I_x/Lel^3), 6*(E*I_x/Lel^2); 
       6*(E*I_x/Lel^2), 4*(E*I_x/Lel), -6*(E*I_x/Lel^2), 2*(E*I_x/Lel);
       -12*(E*I_x/Lel^3), -6*(E*I_x/Lel^2), 12*(E*I_x/Lel^3), -6*(E*I_x/Lel^2); 
       6*(E*I_x/Lel^2), 2*(E*I_x/Lel), -6*(E*I_x/Lel^2), 4*(E*I_x/Lel)];  

% bernoulli beam - y direction
% Lift
M_y = M_x;

K_y = [12*(E*I_y/Lel^3), 6*(E*I_y/Lel^2), -12*(E*I_y/Lel^3), 6*(E*I_y/Lel^2); 
       6*(E*I_y/Lel^2), 4*(E*I_y/Lel), -6*(E*I_y/Lel^2), 2*(E*I_y/Lel);
       -12*(E*I_y/Lel^3), -6*(E*I_y/Lel^2), 12*(E*I_y/Lel^3), -6*(E*I_y/Lel^2); 
       6*(E*I_y/Lel^2), 2*(E*I_y/Lel), -6*(E*I_y/Lel^2), 4*(E*I_y/Lel)];  

% torsional elements
M_t = [(Xt*Lel)/3, (Xt*Lel)/6; (Xt*Lel)/6, (Xt*Lel)/3];

K_t = [(G*J)/Lel, (-G*J)/Lel; (-G*J)/Lel, (G*J)/Lel];


% assembly of structural mass and stiffness matrices for 2-node beam element 
M(1:2, 1:2) = M_x(1:2,1:2);
M(1:2, 6:7) = M_x(1:2, 3:4);
M(3:4, 3:4) = M(1:2, 1:2);
M(3:4, 8:9) = M(1:2, 6:7);
M(5, 5) = M_t(1, 1);
M(5, 10) = M_t(1, 2);
M(6:7, 1:2) = M_x(3:4, 1:2);
M(6:7, 6:7) = M_x(3:4, 3:4);
M(8:9, 3:4) = M(6:7, 1:2);
M(8:9, 8:9) = M(6:7, 6:7);
M(10, 5) = M_t(2, 1);
M(10, 10) = M_t(2, 2);

K(1:2, 1:2) = K_x(1:2,1:2);
K(1:2, 6:7) = K_x(1:2, 3:4);
K(3:4, 3:4) = K_y(1:2, 1:2);
K(3:4, 8:9) = K_y(1:2, 3:4);
K(5, 5) = K_t(1, 1);
K(5, 10) = K_t(1, 2);
K(6:7, 1:2) = K_x(3:4, 1:2);
K(6:7, 6:7) = K_x(3:4, 3:4);
K(8:9, 3:4) = K_y(3:4, 1:2);
K(8:9, 8:9) = K_y(3:4, 3:4);
K(10, 5) = K_t(2, 1);
K(10, 10) = K_t(2, 2);

% assembly of structural mass and stiffness matrices for any number of elements
if n == 1
    M = M;
    K = K;
else
    n_el = (n+1)*5;             % number of elements in matrix
    M_new = zeros(n_el);        % Working variable
    K_new = zeros(n_el);        % Working variable
    
    M_new(1:10, 1:10) = M;
    K_new(1:10, 1:10) = K;
    
    for i=1:n-1
        
        M_new(i*5+1:i*5+10, i*5+1:i*5+10) = M_new(i*5+1:i*5+10, i*5+1:i*5+10) + M;
        K_new(i*5+1:i*5+10, i*5+1:i*5+10) = K_new(i*5+1:i*5+10, i*5+1:i*5+10) + K;
        
    end
    
    
    % Mass and Stifness matrices
    M = M_new;
    K = K_new;
    
end

% assembly of structural damping matrix using Rayleigh damping
C=alpha*M+beta*K;
               
% QUASI-STEADY AERODYNAMIC MODEL

% assembly of aerodynamic stiffness matrix for 2-node beam element
kAM = zeros(10);
kAM(3,5) = cza;
kAM(5,5) = ecza;
kAM(6:10, 6:10) = kAM(1:5, 1:5);

%assemble matrix 
if n == 1;
    kAM = q*kAM;
else
    %if number of elements > 1
    kAM_new = zeros(5*(n+1));
    kAM_new(1:10, 1:10) = kAM(1:10,1:10);
    
    for j = 1:n-1
        kAM_new(i*5+1:i*5+10, i*5+1:i*5+10) = kAM_new(i*5+1:i*5+10, i*5+1:i*5+10) + kAM;
    
    end
    
    kAM = q*kAM_new;
      
end

% assembly of aerodynamic damping matrix for 2-node beam element
cAM = zeros(10);
cAM(3,3) = cza;
cAM(3,5) = ecza;
cAM(5,5) = czM;
cAM(6:10,6:10) = cAM(1:5,1:5);

% assemble matrix
if n == 1;
    cAM = (q/V)*cAM;
else
    %if number of elements > 1
    cAM_new = zeros(5*(n+1));
    cAM_new(1:10, 1:10) = cAM(1:10,1:10);
    
    for j = 1:n-1
        cAM_new(i*5+1:i*5+10, i*5+1:i*5+10) = cAM_new(i*5+1:i*5+10, i*5+1:i*5+10) + cAM;
    
    end
    
    cAM = (q/V)*cAM_new;
    
end


%%% Adding propellor mass and thrust
m_lumped = [ ... 
    m 0 0 0 0; ...
    0 Ix 0 0 0; ...
    0 0 m 0 0; ...
    0 0 0 Iy 0; ...
    0 0 0 0 Jz ];

Kt = [ ... 
    0 0 0 0 0; ...
    0 0 0 0 0; ...
    0 0 0 0 Ft; ...
    0 0 0 0 0; ...
    0 0 0 0 0.5*b*Ft ];

% adding propellor mass and thrust at specified nodes
if isempty(massnodes) == 0   
    for k = 1:length(massnodes)
        mpos = massnodes(k);
        M(5*mpos-4:5*mpos,5*mpos-4:5*mpos) = M(5*mpos-4:5*mpos,5*mpos-4:5*mpos) + m_lumped(1:5,1:5);
        K(5*mpos-4:5*mpos,5*mpos-4:5*mpos) = K(5*mpos-4:5*mpos,5*mpos-4:5*mpos) - Kt(1:5,1:5);
    end
end  


% EQUATIONS OF MOTION - assemble matrices A, B, D
% EoM of the form: A*q_dotdot + B*qdot + D*q = 0
% A = structural intertia (mass matrix, including lumped mass)
% B = combined structural and aerodynamic damping 
% D = combined structural and aerodynamic stiffness, and thrust matrix

% Matrix A
A = M;

% Matrix B
B = C - cAM;

% Matrix D
D = K - kAM;

% Apply boundary conditions
A(:,1:5) = []; % = empty []
B(:,1:5) = [];
D(:,1:5) = [];

A(1:5,:) = [];
B(1:5,:) = [];
D(1:5,:) = [];

%create system matrix
Q = [zeros(length(A)),eye(length(A)); -inv(A)*D, -inv(A)*B];

f_stable = 1;
d_stable = 1;

% for range of airspeeds V_air
for V_air = V_lb:V_int:V_ub
    %V_air;
    
    %sub V into system matrix Q
    syms V
    Q_new = double(vpa(subs(Q,V,V_air)));
    
    % obtain eigenvalues of Q
    eigval = eig(Q_new);
    
    % for each eigenvalue
    for j = 1:length(eigval)
        % if real part of complex eigenvalue is positive
        if isreal(eigval(j)) == 0 && real(eigval(j)) >= 0 && f_stable == 1
            % system is unstable
            f_stable = 0;
            % flutter speed = current airspeed
            Vf = V_air;
            wf=abs(imag(eigval(j)));   % flutter frequency 
        % if real eigenvalue is positive
        elseif isreal(eigval(j)) == 1 && eigval(j) >= 0 && d_stable == 1
            % system is unstable
            d_stable = 0;
            % divergence speed = current airspeed
            Vd = V_air;
        end
    end
end


%%% DISPLAY RESULTS
if f_stable == 1
    disp("System remains dynamically stable within range of airspeeds")
elseif f_stable == 0
    dispVf = ['The flutter speed is ',num2str(Vf),' m/s.'];
    dispwf = ['The flutter frequency is ',num2str(wf),' rad/s.'];
    disp(dispVf)
    disp(dispwf)
end

if d_stable == 1
    disp('')
    disp("System remains statically stable within range of airspeeds")
elseif d_stable == 0
    disp('')
    dispVd = ['The divergence speed is ',num2str(Vd),' m/s.'];
    disp(dispVd)
end

end