clc
clear all
close all
%% initialize parameters
question=1; %(1 or 2 or 3.1 or 3.2)

lib2Kg=0.329;
lib2Kg=0.4536;
lib2N=lib2Kg*9.807;
feet2m=0.3048;

EI_const=2.5*10^6*lib2N*feet2m^2;
GJ_const=0.4*10^6*lib2N*feet2m^2;
Length=(40+80)*feet2m;
n=100;
cl_a=2*pi;
c_const=8*feet2m;
e_const=(0.26-0.25)*c_const;
e2=(0.26-0.25)*c_const;
cl_delta=0;  
cm_delta=0; 
ro=1.225;
deg=-00/180*pi;
mass=6*lib2Kg/feet2m*Length;
v=15; %[m/sev]
w_gag=2; %gust amplitude [m/sec] 
zeta=0.02; %damping coefficient


d=0.00*c_const;
b=c_const/2;
a=(0.26-0.5)*2;
num_modes=4;
relevant_mode=1;




%Q1
delta_m=0;
delta_Ix=0;
delta_m_13=0;
    x_min=0;
    x_max=30;
    gmin=-1;
    gmax=1;
    w_min=0;

    

%% initial calculations
EI=ones(n,1)*EI_const;
GJ=ones(n,1)*GJ_const;
e=ones(n,1)*e_const*0; %e=0 because the aerodynamic model is around the center of rigidity
c=ones(n,1)*c_const;
l=Length/n;
m=mass/n;

%le=l/cos(deg);%effective length in stiffnes matrix- ignored due to Daniela's order
le=l;
S=l*c;
Iz=m*le^2/12+5*lib2Kg*feet2m*le;
Ix=30*lib2Kg*feet2m*le;

s=le*c_const;
delta_m_2_3=50*lib2Kg;
delta_Ix_2_3=delta_m_2_3*3^2*feet2m^2;
delta_Iz_2_3=delta_Ix_2_3;
loc_2_3=fix(80*feet2m/le);

%% generate transtormation matrix (from aerodynamic system to structural)

%build  T_as
T_as=zeros(2*n,3*n);
i=1;
T_as_i=0.5*[-1    0 e(i)    -1    0 e(i)          %e(i)is-0!!!
                    0   0    1      0    0   1];
T_as(1:2,1:3)= T_as(1:2,1:3)+T_as_i(:,4:6);
for i=2:n
    T_as_i=0.5*[-1    0 e(i)    -1    0 e(i)
                    0   0    1      0    0   1];
     T_as(1+2*(i-1):1+2*(i-1)+1,1+3*(i-2):1+3*(i-2)+5)  =    T_as(1+2*(i-1):1+2*(i-1)+1,1+3*(i-2):1+3*(i-2)+5)+T_as_i;
end


%% modal analisyss

%generate  stiffness matrix
K=zeros(3*n,3*n);
i=1;
   k_i=[12*EI(i)/le^3           6*EI(i)/le^2             0                      -12*EI(i)/le^3        6*EI(i)/le^2     0
                6*EI(i)/le^2             4*EI(i)/le                0                      -6*EI(i)/le^2          2*EI(i)/le         0
                0                                0                              GJ(i)/le           0                                0                         -GJ(i)/le
                -12*EI(i)/le^3         -6*EI(i)/le^2          0                      12*EI(i)/le^3           -6*EI(i)/le^2   0
                 6*EI(i)/le^2            2*EI(i)/le               0                      -6*EI(i)/le^2            4*EI(i)/le        0
                 0                                0                             -GJ(i)/le          0                                 0                        GJ(i)/le];
   K(1:3,1:3)= K(1:3,1:3)+k_i(4:6,4:6);
for i=2:n
    k_i=[12*EI(i)/le^3           6*EI(i)/le^2             0                      -12*EI(i)/le^3        6*EI(i)/le^2     0
                6*EI(i)/le^2             4*EI(i)/le                0                      -6*EI(i)/le^2          2*EI(i)/le         0
                0                                0                              GJ(i)/le           0                                0                         -GJ(i)/le
                -12*EI(i)/le^3         -6*EI(i)/le^2          0                      12*EI(i)/le^3           -6*EI(i)/le^2   0
                 6*EI(i)/le^2            2*EI(i)/le               0                      -6*EI(i)/le^2            4*EI(i)/le        0
                 0                                0                             -GJ(i)/le          0                                 0                        GJ(i)/le];
     K(1+3*(i-2):1+3*(i-2)+5,1+3*(i-2):1+3*(i-2)+5)= K(1+3*(i-2):1+3*(i-2)+5,1+3*(i-2):1+3*(i-2)+5)+k_i;
end
%generate mass  matrix 
M=zeros(3*n,3*n);
%i=1;
  m_i=m/2*[1                 l/2                                -d                             0                               0                    0
                    l/2             Iz/m+l^2/4                -d*l/2                      0                                0                    0
                    -d                       -d*l/2         Ix/m+d^2                         0                               0                    0
                       0                        0                    0                                      1                           -l/2                    -d  
                        0                        0                    0                                     -l/2             Iz/m+l^2/4                d*l/2  
                        0                        0                    0                                     -d                       d*l/2         Ix/m+d^2 ];
   M(1:3,1:3)= M(1:3,1:3)+m_i(4:6,4:6);
for i=2:n
     m_i=m/2*[1                 l/2                                -d                             0                               0                    0
                    l/2             Iz/m+l^2/4                -d*l/2                      0                                0                    0
                    -d                       -d*l/2         Ix/m+d^2                         0                               0                    0
                       0                        0                    0                                      1                           -l/2                    -d  
                        0                        0                    0                                     -l/2             Iz/m+l^2/4                d*l/2  
                        0                        0                    0                                     -d                       d*l/2         Ix/m+d^2 ];
     M(1+3*(i-2):1+3*(i-2)+5,1+3*(i-2):1+3*(i-2)+5)= M(1+3*(i-2):1+3*(i-2)+5,1+3*(i-2):1+3*(i-2)+5)+m_i;
end
 
M(loc_2_3*3-2,loc_2_3*3-2)=M(loc_2_3*3-2,loc_2_3*3-2)+delta_m_2_3;
M(loc_2_3*3-1,loc_2_3*3-1)=M(loc_2_3*3-1,loc_2_3*3-1)+delta_Iz_2_3;
M(loc_2_3*3,loc_2_3*3)=M(loc_2_3*3,loc_2_3*3)+delta_Ix_2_3;

[V,D]=eig(K,M); %perform modal analisyss- with this syntax the modes arre automatically normalized to unit mass

WWW=D./2/pi

w=sqrt(D);
w_max=1.2*w(num_modes,num_modes);

%transter modes to aerodynamic C.S

%method 1- Tas
%phi= T_as*V(:,1:num_modes); %leave modes in structural coordinates (take only first ones)

%method 2- erase bending DOFs
phi=V(:,1:num_modes);
for i=2:2:size(V)*2/3
   phi(i,:)=[];
end

%modal solution
GK=D(1:num_modes,1:num_modes); %only correct if modes are normalized to unit mass
GM=eye(num_modes);               %only correct if modes are normalized to unit mass
for i=1:num_modes
    TT(i)=2*pi/sqrt(GK(i,i));
end

MAX_L=12;
for V_index=1:10
for L_index=1:MAX_L

    
v=V_index*3;
lg=TT(relevant_mode)*v*0.5+TT(relevant_mode)*v/MAX_L*L_index*1.5;%gust length [m]
GIST_LENGTH(L_index)=lg;


ssize=300;%*TT(1)/TT(relevant_mode); %num of frequencyis to work with (the higher ones geet a very small coefficient and can be neglected)


phi=V(:,1:num_modes);
for i=2:2:size(V)*2/3
   phi(i,:)=[];
end

q=0.5*ro*v^2;
for i=1:n
    force_vec(2*i-1:2*i)=[1 0]*q*s/v*cl_a;
end

G_F_vec=phi'*force_vec';

G_damping=zeta.*sqrt(GK)*2;
%G_damping(2:num_modes,2:num_modes)=0;    
%G_damping(2:num_modes,2:num_modes)=-0.1*G_damping(2:num_modes,2:num_modes);    
%% generate gust and kussner function folowed by convolution

%lg=40*c_const;
T_gust=lg/v;


w_struct_1=D(1,1); T_struct_1=2*pi/w_struct_1;
w_struct_3=D(2+relevant_mode,2+relevant_mode); T_struct_3=2*pi/w_struct_3;
omega_gust=2*pi/T_gust; 



dt=min(T_struct_3/10,T_gust/10);
T=max(T_gust*10,T_struct_1*10);


size_sig=2^nextpow2(T/dt);

for i=1:size_sig
    t(i)=dt*i-dt;
    if t(i)<T_gust%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        gust(i)=0.5*w_gag*(1-cos(omega_gust*t(i)));
        w_dot(i)=0.5*w_gag*omega_gust*sin(omega_gust*t(i));
    else
        gust(i)=0;
        w_dot(i)=0;
    end
end

for i=1:300
    w_dot(length(w_dot))=[];
    gust(length(gust))=[];
end

w_dot=[zeros(1,300) w_dot];
gust=[zeros(1,300) gust];


s_kussner=v*t/c_const*2;
kussner=(s_kussner.^2+s_kussner)./(s_kussner.^2+2.82*s_kussner+0.8);
cnv=conv(kussner,w_dot)*dt;
ghust_load=cnv(1:length(t)); %
F=ghust_load;

%% transform gust to freqency domain

%omega_gust=sqrt(GK(1,1));
%F=w_gag*sin(omega_gust*t);%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

signal=F;

fs=1/dt; % fs is the sampling frequency
nfft=length(signal);
y=fft(signal,nfft)/nfft;
y=conj(y); %dont know why but the conj gives the correct answer
%y=signal*dftmtx(nfft)/nfft;




y(1)=y(1)/2; %dont know why but the first frequency must b devided by 2 to get correct answer
yy=2*sqrt(y.*conj(y)); % the 2 factor here is in order to get the amplitude peak-to-peak
Amp=yy(1:nfft/2); % here I get rid of all frequencies above half the sampling frequency
Amp=2*abs(y(1:nfft/2));
phase=atan(real(y(1:nfft/2))./imag(y(1:nfft/2)));
f=(0:(nfft/2)-1)/nfft*fs;


sizee=ssize;


f=f(1:sizee);
mekad=y(1:sizee)*2;
figure(011)
plot(f,real(mekad(1:sizee)),f,imag(mekad(1:sizee)),f,abs(mekad(1:sizee)))
legend ('imaginarry part' ,'real part','amplitude','location','best')
grid

w=2*pi*f;




% for i=1:length(phase)
%     if imag(y(i))==0
%         if real(y(i))<pi
%         phase(i)=pi/2;
%         else
%         phase(i)=-pi/2*0; 
%         end
%     end
% end



for i=1:length(t)
    
        FF(i)=real(mekad)*cos(w'*t(i))+imag(mekad)*sin(w'*t(i));
        %FF(i)=Amp*cos(w'*t(i)+phase'/2*2);
        
end

if L_index==4
figure(020+V_index)
plot((t-t(300))*v/lg,gust/w_gag,(t-t(300))*v/lg,F/w_gag,(t-t(300))*v/lg,FF/w_gag)
legend('original gust','gust after kussner', 'gust after kusner filtered and returned' ,'location','best')
grid
xlabel('x/Lg')
ylabel('gust amplitude')
title('comparisson between original gust and gust after kussner filter')
xlim([0,2])
end




% w_gust=[0.1:1:100];
% %CL=1./w_gust;
% CL=zeros(1,length(w_gust));
% CL(3)=20;
% CL(15)=300;


w_gust=f*2*pi;
CL=mekad;

%% solve gust in frequency domain

for i=1:length(w_gust)
   
    kk(i)=w_gust(i)*b/v;
    k=kk(i);
    
    ww=w_gust(i);
    
    if w_gust(i)==0
      ww=w_gust(i+1);
      k=w_gust(i+1)*b/v;
    end

%CC=besselk(1,1i*k)/(besselk(0,1i*k)+besselk(1,1i*k));
CC=besselh(1,2,k)/(besselh(1,2,k)+1i*besselh(0,2,k));
F=real(CC);
G=imag(CC);

l_z=-cl_a*(-k^2/2-k*G);
l_z_d=-cl_a*F;
l_teta=cl_a*(k^2*a/2+F-G*k*(0.5-a));
l_teta_d=cl_a*(0.5+F*(0.5-a)+G/k);
m_z=-cl_a*(-k^2*a/2-G*k*(0.5+a));
m_z_d=-cl_a*(a+0.5)*F;
m_teta=cl_a*(k^2/2*(1/8+a^2)+F*(a+0.5)-k*G*(a+0.5)*(0.5-a));
m_teta_d=cl_a*(-k/2*(0.5-a)+k*F*(a+0.5)*(0.5-a)+G/k*(a+0.5));



B_j=le*[b*l_z_d           b^2*l_teta_d
                
                        b^2*m_z_d            b^3*m_teta_d];

C_j=le*[l_z         b*l_teta
                            
                        b*m_z   b^2*m_teta];



B_i=zeros(2*n,2*n) ;

for j=1:n
      B_i(2*j-1:2*j,2*j-1:2*j)=B_j;
end

C_i=zeros(2*n,2*n) ;
for j=1:n
      C_i(2*j-1:2*j,2*j-1:2*j)=C_j;
end

%transfer C and B to structural coordinates- not done!!
%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
%B_i=T_as'*B_i*T_as; 
%C_i=T_as'*C_i*T_as;

%modal solution (in aerodynamic C.S)
GB_i=phi'*B_i*phi;
GC_i=phi'*C_i*phi;

    ww=w_gust(i);
    kk(i)=w_gust(i)*b/v;

MAT=-ww^2*GM+GK-1i*ww*G_damping+1i*ww*GB_i-GC_i;
force=G_F_vec*CL(i);

xi_freq(i,1:num_modes)=MAT\force;
%xi_freq(i,1)=0;%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!  
end

%% retrieve solution to time domain

for i=1:length(w_gust);
    for j=1:num_modes;
        xi (i,j,1:length(t))=real(xi_freq(i,j))*cos(w_gust(i)*t)+imag(xi_freq(i,j))*sin(w_gust(i)*t);
    end
end
xi_tot=zeros(length(t),num_modes);
for k=1:length(t) 
    for j=1:num_modes
        for i=1:length(w_gust)
             xi_tot(k,j)=xi_tot(k,j)+xi(i,j,k);
        end
    end   
end

delta_st=(0.5*ro*v^2*cl_a*w_gag/v)*(Length^4)/8/EI_const*c_const;

X=phi*xi_tot'/delta_st;

amp_max(L_index,V_index)=max(X(n*2-1,1:length(t)));

figure(V_index+100)
if L_index==1
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'b')
    hold on
end
if L_index==2
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'r')
    hold on
end
if L_index==3
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'c')
    hold on
end
if L_index==4
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'g')
    hold on
end
if L_index==5
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'k')
    hold on
end
if L_index==6
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'m')
    hold on
end
if L_index==7
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--b')
    hold on
end
if L_index==8
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--r')
    hold on
end
if L_index==9
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--c')
    hold on
end
if L_index==10
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--g')
    hold on
end
if L_index==11
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--k')
    hold on
end
if L_index==12
    plot (t/(2*pi/sqrt(GK(1,1))),X(n*2-1,1:length(t)),'--m')
    hold on
end

hold on

disp=X;
for i=1:n
    disp(i,1:length(t))=X(2*i-1,1:length(t));
end

%% solve static problem and dynamic problem for sinus oidal gust (no aerodynamic stiffness and damping)
delta_st=(0.5*ro*v^2*cl_a*w_gag/v)*(Length^4)/8/EI_const*c_const

ll=Length;
syms x 

phi=x^5/ll^5/11+20/11*x^2/ll^2-10/11*x^3/ll^3;
subs(phi,x,ll);
m_l=mass/Length;
keq=double(int(diff(phi,x,2)^2,x,0,ll)*EI_const);
meq=double(int(phi^2,x,0,ll)*m_l+delta_m_2_3*subs(phi,x,2/3*Length)^2);
peq=0.5*ro*v^2*double(w_gag/v*cl_a*c_const*int(phi,x,0,ll));
w=sqrt(keq/meq)


beta=omega_gust/w;
delta_max=peq/(keq*sqrt((1-beta^2)^2+4*zeta^2*beta^2))

end
figure (100+V_index)
grid
legends=GIST_LENGTH/(2*pi/w*v);
legend(num2str(legends(1)),num2str(legends(2)),num2str(legends(3)),num2str(legends(4)),num2str(legends(5)),num2str(legends(6)),num2str(legends(7)),num2str(legends(8)),num2str(legends(9)),num2str(legends(10)),num2str(legends(11)),num2str(legends(12)))
xlabel('t/T_1')
ylabel('max displacement/static displacement')
str1={'normalized: displacements in time @ VT/b=',v/c_const/2*(2*pi/w)};
xlim([0,5])
title (str1)

figure(200+V_index)
plot(GIST_LENGTH/(2*pi/w)/v,amp_max(:,V_index))

xlabel('gust length/structural 1st period length')
ylabel('max displacement/static displacement')
str2={'normalized: displacements Vs gust length @ L_s_t_1/b=',v/c_const/2*(2*pi/w)};
title (str2)
grid
end

max_amplitudes=zeros(V_index,1);
for i=1:V_index
    max_amplitudes(i)=max(amp_max(:,i));
end

vv=(1:1:10)*3;
figure(700)
plot (vv/c_const/2*(2*pi/w),max_amplitudes)
xlabel('VT_1/b=L_s_p_1/b')
ylabel('max displacement/static displacement')
str3={'normalized: max displacement Vs flight speed'};
title (str3)
grid

figure(800)
plot(s_kussner,kussner)
xlabel('s=vt/b')
ylabel('XSI(s)')
grid
title('kussner function')
xlim([0,40])

for V_index=1:10
v=V_index*3; 
figure(200+V_index)
str2={'normalized: displacements Vs gust length',' L_s_p_1/b=' v/c_const/2*(2*pi/w)};
title (str2)
ylim([0.7,1.1])
figure(100+V_index)
str1={'normalized: displacements in time',' L_s_p_1/b=',v/c_const/2*(2*pi/w)};
title (str1)
end
