%% This code generated modal frequency and Modal damping curves for specified values of 
%% structural damping and velocity using various aerodynamic models


clc
clear all
% close all

%% NATA II parameters

load Params_all
rho=1.225;
a=Params.a;
s=Params.s;
b=Params.b;
CLA=Params.CLA;
CMA=Params.CMA;
pitch_dr=Params.Pitch_dr;
plunge_dr=Params.Plunge_dr;
mw=Params.Wing_Mass;
Iw=Params.Wing_Inertia;
xw=Params.Wing_cg;
kal=Params.Pitch_stiffness_lin;
It=Params.Pitch_It;
wa=sqrt(kal/It);
kh=Params.Plunge_stiffness;
mh=Params.Plunge_mass;
wh=sqrt(kh/mh);
moff    =   mw*xw;
cca=2*sqrt(It*kal);
cch=2*sqrt(mh*kh);
ch=plunge_dr*cch;
ca=pitch_dr*cca;


%% Develop Peters matrices

N=6;
D=zeros(N,N);
d=zeros(N,1);
C=d;
B=d;
% f=2;
for n=1:N
    for m=1:N
        C(n,1)=2/n;
        if n==1 d(n,1)=0.5;           else d(n,1)=0;        end
        if n==N 
            B(n,1)=(-1)^(n-1);    
        else
            B(n,1)=((-1)^(n-1))*(factorial(N+n-1) ...
                /factorial(N-n-1)/factorial(n)^2);      
        end
        if n==m+1 D(n,m)= 1/(2*n);  elseif n==m-1 D(n,m)=-1/(2*n); ...
        else D(n,m)=0;              end
    end
end
AL=D + d*B' + C*d' + 0.5*C*B';
% AL=D+ C*d' +0.5*C*B';

%%

dfa=0*pitch_dr;
dfh=0*plunge_dr;

Na=length(dfa);
Nh=length(dfh);
Nk=5000;
for ja=1:Na
    ca(ja)=dfa(ja)*cca;
    for jh=1:Nh
        ch(jh)=dfh(jh)*cch;
        for k=1:Nk
            U(k)=20*k/Nk;       
   
           La=0; Lda=0; Ldda=0;
           Ldh=0; Lddh=0; Ll=zeros(6,1);
           Mddh=0; Mda=0; Mdda=0;
           
           
 %% Calculation of quasi static values
 
            La       =   CLA*rho*b*(U(k))^2*s;
            MLa      =   La*b*(0.5+a); 
            MLda    =   Lda*b*(0.5+a);
            MLdda    =  Ldda*b*(0.5+a);  
            MLdh  =     Ldh*b*(0.5+a);
            MLddh   =   Lddh*b*(0.5+a);
%             MLl         =   Ll*b*(0.5+a);

            A1=[1,       0,          0,          0;...
            0,          mh+Lddh,         0,          moff-Ldda;...
            0,          0,          1,          0;...
            0,          moff-MLddh+Mddh,       0,          It+MLdda+Mdda];
                      
           B1=[0,       1,          0,                      0;...
            -kh,        -ch(jh)-Ldh,         -La,                  -Lda;...
            0,          0,          0,                      1;...
            0,          MLdh,       MLa-kal,          -ca(ja)+MLda-Mda];

            v1(k,:)=eig(inv(A1)*B1);
            Vp1(k,:)=v1(k,:)./wa;
            
            V(k)=U(k)/b/wa;
            
            
%% Unsteady Calculations
            
            La       =   CLA*rho*b*(U(k))^2*s;
            Lda      =   pi*rho*b^2*s*U(k) + CLA*rho*U(k)*b^2*s*(0.5-a);
            Lda1     =   CLA*rho*U(k)*b^2*s*(0.5-a);
            Ldda     =   pi*rho*b^3*a*s;
            Ldh      =   CLA*rho*U(k)*b*s;
            Lddh     =   pi*rho*b^2*s;
            Ll       =   CLA*rho*U(k)*b*0.5*B*s;
            Ll       =   zeros(6,1);


            MLa      =   La*b*(0.5+a); 
            MLda    =   Lda1*b*(0.5+a);
            MLdda    =  0;%Ldda*b*(0.5+a);  
%           MLdda    =  Ldda*b*(0.5+a);  

            MLdh  =     Ldh*b*(0.5+a);
            MLddh   =   0;%Lddh*b*(0.5+a);
%           MLddh   =   Lddh*b*(0.5+a);

            MLl         =   Ll*b*(0.5+a);            
            % 
            Mddh     =   s*pi*rho*(b^3)/2;
            Mda  =   s*pi*rho*b^3*U(k);
            Mdda      =  s*pi*rho*(b^4)*(1/8-a/2);


            A1=[1,       0,          0,          0;...
            0,          mh+Lddh,         0,          moff-Ldda;...
            0,          0,          1,          0;...
            0,          moff-MLddh+Mddh,       0,          It+MLdda+Mdda];
% 
            A2=zeros(4,6);
            A3=[zeros(6,1),-C,-U(k).*C, -b*(0.5-a).*C];
            A4=AL;
            AT=[A1,A2;A3,A4];

            B1=[0,       1,          0,                      0;...
            -kh,        -ch(jh)-Ldh,         -La,                  -Lda;...
            0,          0,          0,                      1;...
            0,          MLdh,       MLa-kal,          -ca(ja)+MLda-Mda];

            B2=[zeros(1,6);Ll';zeros(1,6);-MLl'];
            B3=zeros(6,4);
            B4=-(U(k)/b)*eye(6);

            BT=[B1,B2;B3,B4];

            v2(k,:)=eig(AT\BT);


            Vp2(k,:)=v2(k,:)./wa;
           
            V(k)=U(k)/b/wa;

%             mf1(k)=imag(Vp(k,1));
%             mf2(k)=imag(Vp(k,3));
%             md1(k)=real(Vp(k,1));
%             md2(k)=real(Vp(k,3));
            
            
            
        end
    end
end

figure(1), 

p(1)=subplot(2,1,1); 
hold on
% plot(U,sort(imag(Vp1)'),'-k','LineWidth',2); 
plot(U,sort(imag(Vp2)'),'-k','LineWidth',2); 

ylim([0 1.2]); 
ylabel('Frequency, \Omega/ {w_\alpha}','FontSize',12);
set(gca,'FontSize',12);
grid on
           
p(2)=subplot(2,1,2); 
% figure
hold on
% plot(U,sort(real(Vp1)'),'-k','LineWidth',2);
plot(U,sort(real(Vp2)'),'-k','LineWidth',2); 

line([0 20],[0 0])
ylabel('Damping, \Gamma/ {w_\alpha}','FontSize',12);
xlabel('Freestream Velocity, V (m/s)','FontSize',12);
set(gca,'FontSize',12);
ylim([-0.2 0.2]); 
xlim([0 15])
grid on
linkaxes(p,'x');
title('Effect of structural damping on predicted flutter speed','FontSize',12);

legend
figure(1), subplot(2,1,1)
l1=plot([-1 -1],[-2 -2],'-k','LineWidth',2);
l2=plot([-1 -1],[-2 -2],'-b','LineWidth',2);
l3=plot([-1 -1],[-2 -2],'-r','LineWidth',2);

% l3=plot([-1 -1],[-2 -2],'.-k','LineWidth',2);
legend([l1 l2 l3],'No damping','\zeta_h=0.0416, \zeta_\alpha=0.0322','\zeta_h=0.0832, \zeta_\alpha=0.0644','FontSize',12,'Location','SouthEast');


