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*5
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.25-0.25)*c_const;
cl_delta=0;  
cm_delta=0; 
ro=1.225;
deg=-00/180*pi;
mass=6*lib2Kg/feet2m*Length;


d=0.00*c_const;
b=c_const/2;
a=(0.26-0.5)*2;
num_modes=4;
ll=0.5; %length og bar


k_max=10;
k_min=0.001;
k_interval=0.002;



%Q1
delta_m=0;
delta_Ix=0;
delta_m_13=0;
    x_min=0;
    x_max=50;
    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


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



w=sqrt(D)./2/pi;
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



%straight silution
%GK=T_as*K*T_as';
%GM=T_as*M*T_as';
%%


%i-k...
%j-element
i=0;
for k=k_min:k_interval:k_max
    i=i+1;
    kk(i)=k;
   

%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 D.S)
GB_i=phi'*B_i*phi;
GC_i=phi'*C_i*phi;

%straigh solution-not done!!
%GB_i=B_i;
%GC_i=C_i;


MAT=(GM+1i*ro*(b/k)*GB_i+ro*(b/k)^2*GC_i)/GK; %grnerate solution matrix
%MAT=(GM-1i*ro*(b/k)*GB_i-ro*(b/k)^2*GC_i);
[VV,DD]=eig(MAT);

for ii=1:num_modes
    lamda(ii,i)=DD(ii,ii);
    w_ae(ii,i)=1./sqrt((real(lamda(ii,i))));
    g(ii,i)=imag(lamda(ii,i))./real(lamda(ii,i));
    v(ii,i)=w_ae(ii,i)*b/k;

    
    
end


end


%% plots

figure(question*100+1)
for i=1:num_modes    
plot(v(i,:),w_ae(i,:)/2/pi,'.'),
grid on
hold on
end
xlim([x_min,x_max])
ylim([w_min,w_max])
name=['question-',num2str(question),':  V-W plot'];
title(name)
xlabel ('v [m/sec]')
ylabel ('w [rad/sec]')
hold off


figure(question*100+2)
for i=1:num_modes
plot(v(i,:),g(i,:),'.'),
grid on
hold on
end
xlim([x_min,x_max])
ylim([gmin,gmax])
name=['question-',num2str(question),':  V-g plot'];
title(name)
xlabel ('v [m/sec]')
ylabel ('g [-]')
hold off





