close all 
clear all
clc
disp('------====== FLUTTER ANALYSIS ======------')
disp(' ')
disp(' ')
disp(' Autor: Ales Kratochvil ')
disp(' ales.kratochvil@cvut.cz')
disp(' Institution: Czech Technical University in Prague')
disp(' 2022')
%
%
V=10:2:170;
%                  0 - NE    1 - ANO
vgf=1;             %Tisknout v-g-f diagramy                               
trideni_MSP=1;     %Tridit vl. cisla dle VTK, pouze v def. frek. rozsahu  
Rlocus=1;          %RLOCUS maly tj. ve zvolenem frek.rozsahu   
%
nazev='Flutter analysis';
%                   ISO    Z BOKU   ZE ZADU    ZE SHORA       
x_view = 0; %Default  -1      1          0          0
y_view = 1; %Default  -1      0          1          0
z_view = 0; %Default   1      0          0          1
%
%**** VOP *****     **** SOP *****      **** TRUP ****
EIx_1=2.48E+04;      EIx_2=5.6E+03;      EIx_3=8.895E+5;   %ohybova tuhost
GJp_1=4.65E+03;      GJp_2=5.04E+04;     GJp_3=1.170E+05;   %torzni tuhost
EIy_1=1.0E+06;       EIy_2=1.0E+06;      EIy_3=2.952E+05;   %predozadni tuhost  
EA_vop=1.0E+09;      EA_sop=1.0E+09;     EA_tr=1.0E+09;  %ABS.T.. 
%
ZADANI_vop=[                                                                                                                % *)
%Nod_ID    z      dL     x_p        c      xe     xk       m          S_eo         J_eo       S_k         J_k     delta_J  Elm_ID  Nd@A  Nd@B   OHYB   TORZE    PRED. k_delta   k_delta_T 
1       -1.250	0.156	0.276	0.115	-0.057	-0.038	2.990000	0.675823	0.239818	0.008200	0.023000	0.0     1       1     2     1.00	1.00	1.00	0.0     480
2       -1.094	0.157	0.143	0.209	-0.104	0.049	2.990000	0.535293	0.182896	0.008200	0.023000	0.0     2       2	  3     1.00	1.00	1.00	0.0     480
3       -0.937	0.156	0.105	0.239	-0.119	0.071	2.990000	0.490443	0.167510	0.008200	0.023000	0.0     3       3	  4     1.00	1.00	1.00	0.0     480
4       -0.781	0.156	0.071	0.267	-0.133	0.092	2.990000	0.448583	0.154364	0.008200	0.023000	0.0     4       4	  5     1.00	1.00	1.00	0.0     480
5       -0.625	0.156	0.057	0.282	-0.141	0.098	2.990000	0.426158	0.147803	0.008200	0.023000	0.0     5       5	  6     1.00	1.00	1.00 	0.0     480
6       -0.469	0.156	0.043	0.296	-0.148	0.104	2.990000	0.404480	0.141781	0.008200	0.023000	0.0     6       6	  7     1.00	1.00	1.00	0.0     480
7       -0.313	0.156	0.029	0.311	-0.155	0.112	2.990000	0.382803	0.136073	0.008200	0.023000	0.0     7       7	  8     1.00	1.00	1.00	0.0     480
8       -0.157	0.157	0.014	0.325	-0.163	0.118	2.990000	0.361125	0.130680	0.008200	0.023000	0.0     8       8	  9     1.00	1.00	1.00	0.0     480
9       0.000	0.157	0.000	0.340	-0.170	0.125	2.990000	0.338700	0.125431	0.008200	0.023000    0.2530	9       9	  10	1.00	1.00	1.00	98.4      480
10      0.157	0.156	0.014	0.325	-0.163	0.118	2.990000	0.361125	0.130680	0.008200	0.023000	0.0     10      10	  11	1.00	1.00	1.00	0.0     480
11      0.313	0.156	0.029	0.311	-0.155	0.112	2.990000	0.382803	0.136073	0.008200	0.023000	0.0     11      11	  12	1.00	1.00	1.00  	0.0     480 
12      0.469	0.156	0.043	0.296	-0.148	0.104	2.990000	0.404480	0.141781	0.008200	0.023000	0.0     12      12	  13	1.00	1.00	1.00 	0.0     480
13      0.625	0.156	0.057	0.282	-0.141	0.098	2.990000	0.426158	0.147803	0.008200	0.023000	0.0     13      13	  14	1.00	1.00	1.00 	0.0     480
14      0.781	0.156	0.071	0.267	-0.133	0.092	2.990000	0.448583	0.154364	0.008200	0.023000	0.0     14      14	  15	1.00	1.00	1.00    0.0     480
15      0.937	0.157	0.105	0.239	-0.119	0.071	2.990000	0.490443	0.167510	0.008200	0.023000	0.0     15      15	  16	1.00	1.00	1.00    0.0     480
16      1.094	0.156	0.143	0.209	-0.104	0.049	2.990000	0.535293	0.182896	0.008200	0.023000	0.0     16      16	  17	1.00	1.00	1.00    0.0     480
17      1.250	0.156	0.276	0.115	-0.057	-0.038	2.990000	0.675823	0.239818	0.008200	0.023000	0.0     0       0	  0 	0.00	0.00	0.00	0.0     0
]; 
%    
%
ZADANI_sop=[    
%Nod_ID   y       dL      x_p      c      xe      xk        m       S_eo           J_eo        S_k         J_k     delta_J  Elm_ID  Nd@A   Nd@B  OHYB   TORZE  PRED.  k_delta  k_delta_T                    
9       -1.054	0.072	0.503	0.225	-0.112	0.225	4.971720	0.885104	0.386070	0.000000	0.000000	0       17      9	    18   1.00	1.00	1.00	0       0
18  	-0.982	0.110	0.469	0.374	-0.187	0.090	5.224372	0.467604	0.323455	0.175000	0.057045	0       18      18      19	 1.00	1.00	1.00	0       100000
19      -0.872	0.110	0.416	0.396	-0.198	0.088	5.748953	0.530520	0.313178	0.175000	0.057045	0       19      19      20	 2.00	1.00	1.00	0       100000
20      -0.762	0.110	0.364	0.418	-0.209	0.087	7.509900	0.950586	0.595280	0.175000	0.057045	0       20      20      21	 10.0	1.00	1.00	0       100000
21      -0.652	0.110	0.311	0.440	-0.220	0.086	6.474565	1.055731	0.726005	0.175000	0.057045	0       21      21      22	 20.0	1.00	1.00	0       100000
22      -0.542	0.110	0.259	0.462	-0.231	0.085	6.679150	1.169516	0.761727	0.175000	0.057045	0       22      22      23	 20.0	1.00	1.00	0       100000
23      -0.432	0.110	0.206	0.485	-0.242	0.084	6.699475	1.277787	0.785818	0.175000	0.057045	0       23      23      24	 20.0	1.00	1.00	0       100000
24      -0.322	0.110	0.154	0.507	-0.253	0.084	6.886465	1.400588	0.815775	0.175000	0.057045	0       24      24      25	 10.4	1.00	1.00	0       100000
25      -0.212	0.110	0.101	0.529	-0.264	0.083	23.997350	-0.431260	1.424372	0.175000	0.057045	0       25      25      26	 11.2	1.00	1.00	0       100000
26      -0.102	0.102	0.049	0.551	-0.275	0.082	38.970880	0.303380	1.643110	0.175000	0.057045	0       26      26      27	 10.2	1.00	1.00	0       100000
27      0.000	0.153	0.000	0.571	-0.286	0.080	23.460293	3.871712	2.201518	0.175000	0.057045	0       27      27      28	 11.2	1.00	1.00	0       100000
28      0.153	0.153   -.073	0.600	-0.300	0.081	15.556100	4.895570	2.434609	0.175000	0.057045	0.040	0       0	    0	 0.00	0.00	0.00	358     0
];
%
%
ZADANI_tr=[
%Nod_ID   x      dL     x_p      c     xe       m       S_eo    J_eo        Elm_ID  Nd@A  Nd@B   OHYB    TORZE   PRED. 
27      0       0.171	0.029	0.113	0	6.6094      0       0.125475328	28      27      29   1.00	1.00	1.00
29      -0.171	0.200	0.029	0.113	0	9.1866      0       0.174401859	29      29      30	 1.00	1.00	1.00
30      -0.371	0.200	0.022	0.112	0	7.4946      0       0.139762111	30      30      31	 1.00	1.25	1.00
31      -0.571	0.200	0.013	0.116	0	7.251       0       0.146362258	31      31      32	 1.00	2.50	1.00
32      -0.771	0.200	0.004	0.121	0	5.272       0       0.114832671	32      32      33	 1.00	2.50	1.00
33      -0.971	0.200	-.006	0.125	0	4.674       0       0.109553906	33      33      34	 1.00	2.50	1.00
34      -1.171	0.200	-.015	0.129	0	4.779       0       0.119300993	34      34      35	 1.00	2.50	1.00
35      -1.371	0.200	-.023	0.133	0	4.885       0       0.128630255	35      35      36	 1.00	2.50	1.00
36      -1.571	0.200	-.034	0.139	0	4.981       0       0.143314426	36      36      37	 1.00	2.50	1.00
37      -1.771	0.200	-.047	0.147	0	5.977       0       0.193725765	37      37      38	 1.00	2.50	1.00
38      -1.971	0.200	-.059	0.162	0	5.368       0       0.211332434	38      38      39	 1.00	2.50	1.00
39      -2.171	0.200	-.071	0.184	0	6.061       0       0.307801824	39      39      40	 1.00	2.50	1.00
40      -2.371	0.200	-.080	0.216	0	5.832       0       0.406287171	40      40      41	 1.00	2.50	1.00
41      -2.571	0.200	-.089	0.252	0	7.241       0       0.687023801	41      41      42	 1.00	2.50	1.00
42      -2.771	0.200	-.100	0.297	0	7.241       0       0.958095285	0       0       0	 0.00	0.00	0.00
];
%
%
%,,,
ro=1.225;
alfa1=0.2048;
beta1=0.0557;
alfa2=0.2952;
beta2=0.333;
f_min=1;
f_max=75;
cref=0.34;

% ----------------------------------------
%  ZADANNI PRO POCATECNI PODMINKU SYSTEMU
% ----------------------------------------
%
% Kodova cisla pred aplikaci OP+Nul. sl.
PP_kc=2:9:371; % UY -> VOP+SOP+TR
%
% Amplitudat pocatecniho impulsu dle Kodovych cisel (Rychlost je natvrdno NULOVA)
IMPULS(1,1:28)=0.023*0;  % Nd@VOP+SOP
IMPULS(1,29:42)=0*[0.02081 0.01829 0.01585 0.01349 0.01124 0.00914 0.00719 0.00542 0.00386 0.00254 0.00146 0.00066 0.00017 0];%Nd@TR
%
%
if numel(PP_kc)~=numel(IMPULS)
    disp('ERROR Nespravne def. POCATECNI PODMINKY. Kontrola PP_kc / IMPULS')
end
%
% ----------------------------------------
%  NE-NULOVY VSTUPU DO SYSTEMU
% ----------------------------------------
% Nulovy vstup do systemu 'u=0' dole
%
% Vektor zavedenni vstupu do prislusneho DOF
F=zeros(1,274);
F(1,191)=500;   %kc=191 je Y pro ND@28
%  
%
%--------------------------------------------------------------------------
%                   Extrahovani zadani a vypocet konstant
%--------------------------------------------------------------------------
%       VOP        %
Nod_ID_vop=ZADANI_vop(:,1)';
z_vop=ZADANI_vop(:,2)';
dL_vop=ZADANI_vop(:,3)';
x_p_vop=ZADANI_vop(:,4)';
c_vop=ZADANI_vop(:,5)';
xe_vop=ZADANI_vop(:,6)';
xk_vop=ZADANI_vop(:,7)';
m_vop=ZADANI_vop(:,8)';
S_eo_vop=ZADANI_vop(:,9)';
J_eo_vop=ZADANI_vop(:,10)';
S_k_vop=ZADANI_vop(:,11)';
J_k_vop=ZADANI_vop(:,12)'+ZADANI_vop(:,13)';
% J_delta_vop
Elm_vop=ZADANI_vop(:,14)';   % NEPOTREBUJI ZATIM
NdA_vop=ZADANI_vop(:,15)';
NdB_vop=ZADANI_vop(:,16)';
EIx_vop=ZADANI_vop(:,17)'*EIx_1; %OHYB
GJp_vop=ZADANI_vop(:,18)'*GJp_1; %TORZE
EIy_vop=ZADANI_vop(:,19)'*EIy_1; %PRED.
k_delta_vop=ZADANI_vop(:,20)';
k_delta_T_vop=ZADANI_vop(:,21)';
%
pocet_nodu_vop=numel(Nod_ID_vop);
pocet_elem_vop=pocet_nodu_vop-1;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       SOP           %
Nod_ID_sop=ZADANI_sop(:,1)';
y_sop=ZADANI_sop(:,2)';
dL_sop=ZADANI_sop(:,3)';
x_p_sop=ZADANI_sop(:,4)';
c_sop=ZADANI_sop(:,5)';
xe_sop=ZADANI_sop(:,6)';
xk_sop=ZADANI_sop(:,7)';
m_sop=ZADANI_sop(:,8)';
S_eo_sop=ZADANI_sop(:,9)';
J_eo_sop=ZADANI_sop(:,10)';
S_k_sop=ZADANI_sop(:,11)';
J_k_sop=ZADANI_sop(:,12)'+ZADANI_sop(:,13)';
% J_delta_vop
Elm_sop=ZADANI_sop(:,14)';      % NEPOTREBUJI ZATIM
NdA_sop=ZADANI_sop(:,15)';
NdB_sop=ZADANI_sop(:,16)';
EIx_sop=ZADANI_sop(:,17)'*EIx_2; %OHYB
GJp_sop=ZADANI_sop(:,18)'*GJp_2; %TORZE
EIy_sop=ZADANI_sop(:,19)'*EIy_2; %PRED.
k_delta_sop=ZADANI_sop(:,20)';
k_delta_T_sop=ZADANI_sop(:,21)';
%
pocet_nodu_sop=numel(Nod_ID_sop);
pocet_elem_sop=pocet_nodu_sop-1;
%
% Transformacni matice pro tuhosti R_sop a hmotnosti R_sop2
r_sop=[1 0 0;0 0 1; 0 -1 0];
R_sop=[r_sop zeros(3) zeros(3) zeros(3) zeros(3) zeros(3);
    zeros(3) r_sop zeros(3) zeros(3) zeros(3) zeros(3);
    zeros(3) zeros(3) r_sop zeros(3) zeros(3) zeros(3);
    zeros(3) zeros(3) zeros(3) r_sop zeros(3) zeros(3);
    zeros(3) zeros(3) zeros(3) zeros(3) r_sop zeros(3);
    zeros(3) zeros(3) zeros(3) zeros(3) zeros(3) r_sop];
R_sop2=[r_sop zeros(3) zeros(3); zeros(3) r_sop zeros(3); zeros(3) zeros(3) r_sop];
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%       TRUP        %
Nod_ID_tr=ZADANI_tr(:,1)';
x_tr=ZADANI_tr(:,2)';
dL_tr=ZADANI_tr(:,3)';
x_p_tr=ZADANI_tr(:,4)';
c_tr=ZADANI_tr(:,5)';
xe_tr=ZADANI_tr(:,6)';
m_tr=ZADANI_tr(:,7)';
S_eo_tr=ZADANI_tr(:,8)';
J_eo_tr=ZADANI_tr(:,9)';
Elm_tr=ZADANI_tr(:,10)';      % NEPOTREBUJI ZATIM
NdA_tr=ZADANI_tr(:,11)';
NdB_tr=ZADANI_tr(:,12)';
EIx_tr=ZADANI_tr(:,13)'*EIx_3; %OHYB
GJp_tr=ZADANI_tr(:,14)'*GJp_3; %TORZE
EIy_tr=ZADANI_tr(:,15)'*EIy_3; %PRED.
%
pocet_nodu_tr=numel(Nod_ID_tr);
pocet_elem_tr=pocet_nodu_tr-1;
%
S_k_tr=zeros(1,pocet_nodu_tr);         % Absence prvku v zadani 
J_k_tr=zeros(1,pocet_nodu_tr);         % Absence prvku v zadani 
k_delta_tr=zeros(1,pocet_nodu_tr);     % Absence prvku v zadani 
xk_tr=zeros(1,pocet_nodu_tr);          % Absence prvku v zadani 
%
% Transformacni matice pro tuhosti R_tr a hmotnosti R_tr2
r_tr=[0 0 1;0 1 0; -1 0 0];
R_tr=[r_tr zeros(3) zeros(3) zeros(3) zeros(3) zeros(3);
    zeros(3) r_tr zeros(3) zeros(3) zeros(3) zeros(3);
    zeros(3) zeros(3) r_tr zeros(3) zeros(3) zeros(3);
    zeros(3) zeros(3) zeros(3) r_tr zeros(3) zeros(3);
    zeros(3) zeros(3) zeros(3) zeros(3) r_tr zeros(3);
    zeros(3) zeros(3) zeros(3) zeros(3) zeros(3) r_tr];
R_tr2=[r_tr zeros(3) zeros(3); zeros(3) r_tr zeros(3); zeros(3) zeros(3) r_tr];
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
pocet_nodu=numel(c_vop)+numel(c_sop)+numel(c_tr)-2;
%
%  Nd@id DOF;
OP=[pocet_nodu 1;
    pocet_nodu 2;
    pocet_nodu 3;
    pocet_nodu 4;
    pocet_nodu 5;
    pocet_nodu 6];
%
omega_max=2*pi*f_max;
omega_min=2*pi*f_min;
%--------------------------------------------------------------------------
%              SKLADANI STRUKTURALNI MATICE TUHOSTI --- VOP
%--------------------------------------------------------------------------
%% Popis1
Kgl=zeros(9*pocet_nodu);
%
%KODOVA CISLA
for n=1:pocet_elem_vop
CODE_VOP(n,:)=[(NdA_vop(n)*9)-8:(NdA_vop(n)*9),(NdB_vop(n)*9)-8:(NdB_vop(n)*9)];
end
%
% SKLADANI DO GLL MATICE TUHOSTI
for n=1:pocet_elem_vop
    kc=CODE_VOP(n,:);       % kodova cisla n-teho elementu
    %
    A1=EA_vop/dL_vop(n);
    A2=12*EIy_vop(n)/dL_vop(n)^3;
    A3=12*EIx_vop(n)/dL_vop(n)^3;
    A4=6*EIy_vop(n)/dL_vop(n)^2;
    A5=6*EIx_vop(n)/dL_vop(n)^2;
    A6=GJp_vop(n)/dL_vop(n);
    A7=4*EIy_vop(n)/dL_vop(n);
    A8=4*EIx_vop(n)/dL_vop(n);
    A9=k_delta_T_vop(n)/dL_vop(n);
    %
    Kelm=[A2  0   0   0    A4   0   0   0    0    -A2 0   0   0    A4   0   0   0    0;
          0   A3  0   -A5  0    0   0   0    0    0   -A3 0   -A5  0    0   0   0    0;
          0   0   A1  0    0    0   0   0    0    0   0   -A1 0    0    0   0   0    0;
          0   -A5 0   A8   0    0   0   0    0    0   A5  0   A8/2 0    0   0   0    0;
          A4  0   0   0    A7   0   0   0    0    -A4 0   0   0    A7/2 0   0   0    0; 
          0   0   0   0    0    A6  0   0    0    0   0   0   0    0    -A6 0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    A9   0   0   0   0    0    0   0   0    -A9;
          -A2 0   0   0    -A4  0   0   0    0    A2  0   0   0    -A4  0   0   0    0;
          0   -A3 0   A5   0    0   0   0    0    0   A3  0   A5   0    0   0   0    0;
          0   0   -A1 0    0    0   0   0    0    0   0   A1  0    0    0   0   0    0;
          0   -A5 0   A8/2 0    0   0   0    0    0   A5  0   A8   0    0   0   0    0;
          A4  0   0   0    A7/2 0   0   0    0    -A4 0   0   0    A7   0   0   0    0; 
          0   0   0   0    0    -A6  0  0    0    0   0   0   0    0    A6  0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    -A9  0   0   0   0    0    0   0   0    A9];
    %
    % Transformace - neni treba
    Kgl(kc,kc)=Kgl(kc,kc)+Kelm;   % nacti lokalni matici k-teho elementu do GLL matice
end
%
%--------------------------------------------------------------------------
%              SKLADANI STRUKTURALNI MATICE TUHOSTI --- SOP
%--------------------------------------------------------------------------
%
%KODOVA CISLA
for n=1:pocet_elem_sop
CODE_SOP(n,:)=[(NdA_sop(n)*9)-8:(NdA_sop(n)*9),(NdB_sop(n)*9)-8:(NdB_sop(n)*9)];
end
%
%
%
% SKLADANI DO GLL MATICE TUHOSTI
for n=1:pocet_elem_sop
    kc=CODE_SOP(n,:);       % kodova cisla n-teho elementu
    %
    A1=EA_sop/dL_sop(n);
    A2=12*EIy_sop(n)/dL_sop(n)^3;
    A3=12*EIx_sop(n)/dL_sop(n)^3;
    A4=6*EIy_sop(n)/dL_sop(n)^2;
    A5=6*EIx_sop(n)/dL_sop(n)^2;
    A6=GJp_sop(n)/dL_sop(n);
    A7=4*EIy_sop(n)/dL_sop(n);
    A8=4*EIx_sop(n)/dL_sop(n);
    A9=k_delta_T_sop(n)/dL_sop(n);
    %
    Kelm=[A2  0   0   0    A4   0   0   0    0    -A2 0   0   0    A4   0   0   0    0;
          0   A3  0   -A5  0    0   0   0    0    0   -A3 0   -A5  0    0   0   0    0;
          0   0   A1  0    0    0   0   0    0    0   0   -A1 0    0    0   0   0    0;
          0   -A5 0   A8   0    0   0   0    0    0   A5  0   A8/2 0    0   0   0    0;
          A4  0   0   0    A7   0   0   0    0    -A4 0   0   0    A7/2 0   0   0    0; 
          0   0   0   0    0    A6  0   0    0    0   0   0   0    0    -A6 0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    A9   0   0   0   0    0    0   0   0    -A9;
          -A2 0   0   0    -A4  0   0   0    0    A2  0   0   0    -A4  0   0   0    0;
          0   -A3 0   A5   0    0   0   0    0    0   A3  0   A5   0    0   0   0    0;
          0   0   -A1 0    0    0   0   0    0    0   0   A1  0    0    0   0   0    0;
          0   -A5 0   A8/2 0    0   0   0    0    0   A5  0   A8   0    0   0   0    0;
          A4  0   0   0    A7/2 0   0   0    0    -A4 0   0   0    A7   0   0   0    0; 
          0   0   0   0    0    -A6  0  0    0    0   0   0   0    0    A6  0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    -A9  0   0   0   0    0    0   0   0    A9];
    %
    % Transformace
    Kelm_T=R_sop'*Kelm*R_sop;
    Kgl(kc,kc)=Kgl(kc,kc)+Kelm_T;   % nacti lokalni matici k-teho elementu do GLL matice
end
%
%--------------------------------------------------------------------------
%              SKLADANI STRUKTURALNI MATICE TUHOSTI --- TRUP
%--------------------------------------------------------------------------
%
%KODOVA CISLA
for n=1:pocet_elem_tr
CODE_TR(n,:)=[(NdA_tr(n)*9)-8:(NdA_tr(n)*9),(NdB_tr(n)*9)-8:(NdB_tr(n)*9)];
end
%
%
%
%
% SKLADANI DO GLL MATICE TUHOSTI
for n=1:pocet_elem_tr
    kc=CODE_TR(n,:);       % kodova cisla n-teho elementu
    %
    A1=EA_tr/dL_tr(n);
    A2=12*EIy_tr(n)/dL_tr(n)^3;
    A3=12*EIx_tr(n)/dL_tr(n)^3;
    A4=6*EIy_tr(n)/dL_tr(n)^2;
    A5=6*EIx_tr(n)/dL_tr(n)^2;
    A6=GJp_tr(n)/dL_tr(n);
    A7=4*EIy_tr(n)/dL_tr(n);
    A8=4*EIx_tr(n)/dL_tr(n);
    A9=0;
    %
    Kelm=[A2  0   0   0    A4   0   0   0    0    -A2 0   0   0    A4   0   0   0    0;
          0   A3  0   -A5  0    0   0   0    0    0   -A3 0   -A5  0    0   0   0    0;
          0   0   A1  0    0    0   0   0    0    0   0   -A1 0    0    0   0   0    0;
          0   -A5 0   A8   0    0   0   0    0    0   A5  0   A8/2 0    0   0   0    0;
          A4  0   0   0    A7   0   0   0    0    -A4 0   0   0    A7/2 0   0   0    0; 
          0   0   0   0    0    A6  0   0    0    0   0   0   0    0    -A6 0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    A9   0   0   0   0    0    0   0   0    -A9;
          -A2 0   0   0    -A4  0   0   0    0    A2  0   0   0    -A4  0   0   0    0;
          0   -A3 0   A5   0    0   0   0    0    0   A3  0   A5   0    0   0   0    0;
          0   0   -A1 0    0    0   0   0    0    0   0   A1  0    0    0   0   0    0;
          0   -A5 0   A8/2 0    0   0   0    0    0   A5  0   A8   0    0   0   0    0;
          A4  0   0   0    A7/2 0   0   0    0    -A4 0   0   0    A7   0   0   0    0; 
          0   0   0   0    0    -A6  0  0    0    0   0   0   0    0    A6  0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    0    0   0   0   0    0    0   0   0    0;
          0   0   0   0    0    0   0   0    -A9  0   0   0   0    0    0   0   0    A9];
    %
    % Transformace
    Kelm_T=R_tr'*Kelm*R_tr;
    Kgl(kc,kc)=Kgl(kc,kc)+Kelm_T;   % nacti lokalni matici k-teho elementu do GLL matice
end
%
%
clear A1 A2 A3 A4 A5 A6 A7 A8 A9
%
%--------------------------------------------------------------------------
%              SKLADANI STRUKTURALNI MATICE HMOTNOSTI
%--------------------------------------------------------------------------
Ms=zeros(9*pocet_nodu);
%
m=[m_vop,m_sop,m_tr];
S_eo=[S_eo_vop,S_eo_sop,S_eo_tr];
J_eo=[J_eo_vop,J_eo_sop,J_eo_tr];
S_k=[S_k_vop,S_k_sop,S_k_tr];
J_k=[J_k_vop,J_k_sop,J_k_tr];
xe=[xe_vop,xe_sop,xe_tr];
xk=[xk_vop,xk_sop,xk_tr];
Nod_ID=[Nod_ID_vop,Nod_ID_sop,Nod_ID_tr];
dL=[dL_vop,dL_sop,dL_tr];
%
% SKLADANI DO GLL MATICE HMOTNOSTI
for n=1:numel(Nod_ID)                % Poradove cislo elementu
    kc=(Nod_ID(n)*9)-8:Nod_ID(n)*9;  % kodova cisla n-teho Nodu 
    %
    if n<=pocet_nodu_vop+pocet_nodu_sop
        p=1.0;
    else
        p=0.1;
    end
    %
    KVK=(xk(n)-xe(n))*S_k(n)+J_k(n);
    %
    Mnd=dL(n)*[m(n) 0       0    0           0         0       0 0 0;
               0    m(n)    0    0           0         S_eo(n) 0 0 S_k(n);
               0    0       m(n) 0           0         0       0 0 0;
               0    0       0    0.1*J_eo(n) 0         0       0 0 0;
               0    0       0    0           p*J_eo(n) 0       0 0 0;
               0    S_eo(n) 0    0           0         J_eo(n) 0 0 KVK;
               0    0       0    0           0         0       0 0 0; 
               0    0       0    0           0         0       0 0 0;
               0    S_k(n)  0    0           0         KVK     0 0 J_k(n)];
            
    %
    if n<=pocet_nodu_vop
        Mnd_T=Mnd;                  % Transformace  VOP
    elseif n>pocet_nodu_vop && n<=(pocet_nodu_vop+pocet_nodu_sop)
        Mnd_T=R_sop2'*Mnd*R_sop2;   % Transformace  SOP
    else 
        Mnd_T=R_tr2'*Mnd*R_tr2;     % Transformace  Trup
    end    
    %   
    %
    Ms(kc,kc)=Ms(kc,kc)+Mnd_T;    % nacti lokalni matici k-teho elementu do GLL matice
 end
%
%
%
%--------------------------------------------------------------------------
%          PRIDANI TRASY RIZENI DO STRUKTURALNI MATICE TUHOSTI
%--------------------------------------------------------------------------
%
K_rizeni=zeros(9*pocet_nodu);
k_delta=[k_delta_vop,k_delta_sop,k_delta_tr];
%
for n=1:numel(Nod_ID)
       kc=Nod_ID(n)*9;
       %
    if n<=pocet_nodu_vop    %Pro nody VOP
        K_rizeni(kc,kc)=k_delta(n);
    elseif n>pocet_nodu_vop && n<=(pocet_nodu_vop+pocet_nodu_sop) %Pro nody SOP
        K_rizeni(kc-1,kc-1)=k_delta(n);
    else
      nic=0;  % Pro nody TRUPu nedelej nic
    end
end

%
Ks=Kgl+K_rizeni;
%
%
%
%--------------------------------------------------------------------------
%          GENEROVANI REDUKCNI MATICE  [Kz]
%--------------------------------------------------------------------------    
%
radky_Kz=2*9*(pocet_nodu_vop+pocet_nodu_sop-1);
sloupce_Kz=3*9*(pocet_nodu_vop+pocet_nodu_sop+pocet_nodu_tr-2);
Kz=zeros(radky_Kz,sloupce_Kz);
%
%
for n=1:(pocet_nodu_vop+pocet_nodu_sop-1)
    %
    kc_radku_Kz=(1:9)+((n-1)*18);          % Kodova cisla redukcni matice    
    kc_sloupce_Kz=(1:9)+((n-1)*9);        % Kodova cisla redukcni matice    
    kc2_radku_Kz=kc_radku_Kz+9;
    kc2_sloupce_Kz=kc_sloupce_Kz+9*(pocet_nodu_vop+pocet_nodu_sop+pocet_nodu_tr-2);
    %
    Kz(kc_radku_Kz,kc_sloupce_Kz)=eye(9);  % REDUKCNI MATICE
    Kz(kc2_radku_Kz,kc2_sloupce_Kz)=eye(9);
end
%
%
%
%--------------------------------------------------------------------------
% GENEROVANI AERODYNAMICKE MATICE - [Ma]
%                                  - Neni zavisla na rychlsoti
%--------------------------------------------------------------------------
%
rad_modelu=9*pocet_nodu;
%
Ma=zeros(rad_modelu);
%
c=[c_vop,c_sop,c_tr];
xe_=xe./c;
xk_=xk./c;
%
% SKLADANI MATIC
for n=1:pocet_nodu_vop+pocet_nodu_sop  % Poradove cislo Nodu
    kc=(Nod_ID(n)*9)-8:Nod_ID(n)*9;    % kodova cisla n-teho Nodu 
    %
    T1=-(1/3)*sqrt(1-xk_(n)^2)*(2+xk_(n)^2)+xk_(n)*acos(xk_(n));
    T3=-((1/8)+xk_(n)^2)*(acos(xk_(n)))^2+(xk_(n)/4)*sqrt(1-xk_(n)^2)*acos(xk_(n))*(7+2*xk_(n)^2)-(1/8)*(1-xk_(n)^2)*(5*xk_(n)^2+4);
    T7=-1*((1/8)+xk_(n)^2)*acos(xk_(n))+(1/8)*xk_(n)*sqrt(1-xk_(n)^2)*(7+2*xk_(n)^2);
    T13=0.5*(-T7-(xk_(n)-xe_(n))*T1);
    %
     Ma_nd=dL(n)*[0    0                   0  0  0  0                             0 0 0;
                  0    -ro*c(n)^2*pi       0  0  0  ro*c(n)^3*pi*xe_(n)           0 0 ro*c(n)^3*T1;
                  0    0                   0  0  0  0                             0 0 0;
                  0    0                   0  0  0  0                             0 0 0;
                  0    0                   0  0  0  0                             0 0 0;
                  0    ro*c(n)^3*xe_(n)*pi 0  0  0  -ro*c(n)^4*pi*(1/8+xe_(n)^2)  0 0 ro*c(n)^4*(T7+(xk_(n)-xe_(n))*T1);
                  0    0                   0  0  0  0                             0 0 0; 
                  0    0                   0  0  0  0                             0 0 0;
                  0    ro*c(n)^3*T1        0  0  0  -ro*c(n)^4*2*T13              0 0 ro*c(n)^4*(1/pi)*T3];
%
    if n<=pocet_nodu_vop
        Ma_nd_T=Ma_nd;     % Transformace  VOP
    else
        Ma_nd_T=R_sop2'*Ma_nd*R_sop2;   % Transformace  SOP
    end    
    %   
    Ma(kc,kc)=Ma(kc,kc)+Ma_nd_T;    % nacti lokalni matici k-teho elementu do GLL matice
end


%--------------------------------------------------------------------------
%                APLIKACE  OKRAJOVYCH PODMINEK 
%               A MAZANI NULOVYCH RADKU/SLOUPCU       
%               pro matice NEzavisle na rychlosti
%--------------------------------------------------------------------------
%
kc_OP=(OP(:,1)-1)*9+OP(:,2);  
kc_OP=kc_OP';
%
%
% ****** Nalezeni Nulovych sloupcu ****************************************
Ks_abs=abs(Ks);               % Vytvorim matici tuhosti z absolutnich hodnot aby se mi nektere sloupce pri scitani nevynulovaly
SUMA=sum(Ks_abs,1);            % Soucet hodnot matice Kgl v kazdem sloupci
Nulove_sloupce=find(SUMA==0);   % Vrati kodova cisla radku ktere jsou nulove
% ****** Sestaveni vektoru OP resp. kodovych cisel  **************
Vektor_OP=[kc_OP Nulove_sloupce];
%
%
% ****** APLIKACE Vektoru_OP na [Ks] [Ms] [Ma] ***************************
R=rad_modelu; %radky Kgl
S=R;            %sloupce Kgl, matice je ctvercova vzdy
x=1;
y=1;
%
for u=1:R
    if u~=Vektor_OP
        y=1;      
        for p=1:S
        if p~=Vektor_OP 
            Ks_op(x,y)=Ks(u,p);  %Na strukturalni matici tuhosti
            Ms_op(x,y)=Ms(u,p);  %Na strukturalni matici hmotnosti
            %
            Ma_op(x,y)=Ma(u,p);  %Na aerodynamickou matici hmotnosti
            %
            y=y+1;
        end
       end
      x=x+1;
    end
end
%
% ****** APLIKACE Vektoru_OP na [Kz] Redukcni Matici *********************
%
% Kodova cisla pro vyskrtnuti
m=1;
TRUP=[EIy_3; EIx_3; EA_tr; EIx_3; EIy_3; GJp_3; 0; 0; 0];
zbytek=numel(find(TRUP==0));  

for n=1:(numel(Nulove_sloupce)-zbytek)
        kc_Kz_sloupce(m)=Nulove_sloupce(n);   
        kc_Kz_sloupce(m+1)=Nulove_sloupce(n)+(pocet_nodu*9);                                         
        kc_Kz_sloupce(m+2)=Nulove_sloupce(n)+2*(pocet_nodu*9);                                        
        m=m+3;
   
end
Dodatek1=9*(pocet_nodu-1)+1:1:(9*pocet_nodu);  % resp. OP
Dodatek2=Dodatek1+(pocet_nodu*9);
Dodatek3=Dodatek1+2*(pocet_nodu*9);
Vektor_OP_Kz_sloupce=sort([kc_Kz_sloupce, Dodatek1, Dodatek2, Dodatek3]);
%
%
m=1;
for n=1:(numel(Nulove_sloupce)-zbytek*pocet_elem_tr)
    Nd_ID_minus_1=floor((Nulove_sloupce(n)-1)/9);
    kc_Kz_radky(m)=(1+Nd_ID_minus_1*18)+(Nulove_sloupce(n)-Nd_ID_minus_1*9-1);     
    kc_Kz_radky(m+1)=kc_Kz_radky(m)+9;                                             
    m=m+2;
end
Vektor_OP_Kz_radky=sort(kc_Kz_radky);
%
%
R=radky_Kz;     %radky Kz
S=sloupce_Kz;   %sloupce Kz
x=1;
y=1;
for u=1:R
    if u~=Vektor_OP_Kz_radky
        y=1;      
        for p=1:S
        if p~=Vektor_OP_Kz_sloupce 
            Kz_op(x,y)=Kz(u,p);  %Na redukcni matici
            %
            y=y+1;
        end
       end
      x=x+1;
    end
end
%
%
%






%% Popis 2
%--------------------------------------------------------------------------
% GENEROVANI AERODYNAMICKYCH MATIC - [Ka], [Ta], [K3]
%                                  - [A2], [B2], [C2], [D2]
%                                  - bez TRUPU
%--------------------------------------------------------------------------
%
rad_modelu=9*pocet_nodu;
radky_A2=2*(pocet_nodu_vop+pocet_nodu_sop);
sloupce_B2=2*9*(pocet_nodu_vop+pocet_nodu_sop-1);
radky_C2=pocet_nodu_vop+pocet_nodu_sop;
%
rr=1;  % pro hledani rychlosti flutteru
V_flutter=0;
%
for pocitadlo=1:numel(V)
    v=V(pocitadlo)

Ka=zeros(rad_modelu);
Ta=zeros(rad_modelu);
A2=zeros(radky_A2);
B2=zeros(radky_A2,sloupce_B2);
C2=zeros(radky_C2);
D2=zeros(radky_C2,sloupce_B2);
K3=zeros(rad_modelu,radky_C2);
%
%
% SKLADANI MATIC
for n=1:pocet_nodu_vop+pocet_nodu_sop  % Poradove cislo Nodu
    kc=(Nod_ID(n)*9)-8:Nod_ID(n)*9;    % kodova cisla n-teho Nodu 
    %
    kc_B2_radky=n*2;
    kc_B2_sloupce=(Nod_ID(n)*18-17):(Nod_ID(n)*18);
    %
    kc_D2_radky=n;
    kc_D2_sloupce=kc_B2_sloupce;
    %
    kc_K3_radky=(Nod_ID(n)*9-8):(Nod_ID(n)*9);
    kc_K3_sloupce=n;
    %
    T1=-(1/3)*sqrt(1-xk_(n)^2)*(2+xk_(n)^2)+xk_(n)*acos(xk_(n));
    T3=-((1/8)+xk_(n)^2)*(acos(xk_(n)))^2+(xk_(n)/4)*sqrt(1-xk_(n)^2)*acos(xk_(n))*(7+2*xk_(n)^2)-(1/8)*(1-xk_(n)^2)*(5*xk_(n)^2+4);
    T4=-acos(xk_(n))+xk_(n)*sqrt(1-xk_(n)^2);
    T5=-1*(1-xk_(n)^2)-(acos(xk_(n)))^2+2*xk_(n)*sqrt(1-xk_(n)^2)*acos(xk_(n));
    T7=-1*((1/8)+xk_(n)^2)*acos(xk_(n))+(1/8)*xk_(n)*sqrt(1-xk_(n)^2)*(7+2*xk_(n)^2);
    T8=(-1/3)*sqrt(1-xk_(n)^2)*(2*xk_(n)^2+1)+xk_(n)*acos(xk_(n));
    T9=0.5*((1/3)*(sqrt(1-xk_(n)^2))^3+xe_(n)*T4);
    T10=sqrt(1-xk_(n)^2)+acos(xk_(n));
    T11=acos(xk_(n))*(1-2*xk_(n))+sqrt(1-xk_(n)^2)*(2-xk_(n));
    T12=sqrt(1-xk_(n)^2)*(2+xk_(n))-acos(xk_(n))*(2*xk_(n)+1);
    T13=0.5*(-T7-(xk_(n)-xe_(n))*T1);
    %
     Ta_nd=dL(n)*[0    0  0  0  0  0                                        0 0 0;
                  0    0  0  0  0  -ro*c(n)^2*v*pi                          0 0 ro*c(n)^2*v*T4;
                  0    0  0  0  0  0                                        0 0 0;
                  0    0  0  0  0  0                                        0 0 0;
                  0    0  0  0  0  0                                        0 0 0;
                  0    0  0  0  0  -ro*c(n)^3*pi*(0.5-xe_(n))*v             0 0 -ro*c(n)^3*v*(T1-T8-(xk_(n)-xe_(n))*T4+0.5*T11);
                  0    0  0  0  0  0                                        0 0 0; 
                  0    0  0  0  0  0                                        0 0 0;
                  0    0  0  0  0  -ro*c(n)^3*v*(-2*T9-T1+T4*(xe_(n)-0.5))  0 0 ro*c(n)^3*(1/2/pi)*v*T4*T11];           
%
%
     Ka_nd=dL(n)*[0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 -ro*c(n)^2*v^2*(T4+T10);
                  0    0  0  0  0  0  0 0 0; 
                  0    0  0  0  0  0  0 0 0;
                  0    0  0  0  0  0  0 0 -ro*c(n)^2*(1/pi)*v^2*(T5-T4*T10)];           
%
%
    B2_nd=[0 0 0 0 0 1 0 0 T10/pi 0 1/v 0 0 0 c(n)*(0.5-xe_(n))/v  0 0 c(n)*T11/2/pi/v];
    Fac_nd=dL(n)*[0; -2*pi*ro*v^2*c(n); 0; 0; 0; 2*pi*ro*v^2*c(n)^2*(xe_(n)+1/2); 0; 0; -ro*v^2*c(n)^2*T12];    
%
%
    if n<=pocet_nodu_vop
        Ta_nd_T=Ta_nd;      % Transformace  VOP
        Ka_nd_T=Ka_nd;
        B2_nd_T=B2_nd;
        Fac_nd_T=Fac_nd;
           else
        Ta_nd_T=R_sop2'*Ta_nd*R_sop2;   % Transformace  SOP
        Ka_nd_T=R_sop2'*Ka_nd*R_sop2;
        B2_nd_T=B2_nd*R_sop;
        Fac_nd_T=R_sop2'*Fac_nd;
    end    
    %   
    %
    Ta(kc,kc)=Ta(kc,kc)+Ta_nd_T;    % nacti lokalni matici k-teho elementu do GLL matice
    Ka(kc,kc)=Ka(kc,kc)+Ka_nd_T;
    B2(kc_B2_radky,kc_B2_sloupce)=B2(kc_B2_radky,kc_B2_sloupce)+B2_nd_T;
    D2(kc_D2_radky,kc_D2_sloupce)=D2(kc_D2_radky,kc_D2_sloupce)+B2_nd_T/2;
    K3(kc_K3_radky,kc_K3_sloupce)=K3(kc_K3_radky,kc_K3_sloupce)+Fac_nd_T;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    kc_A2=(2*n-1):(2*n);      % Kodova cisla matice A2
    A2(kc_A2,kc_A2)=[0  1;    % Matice A2
                    -beta1*beta2*(v/c(n))^2   -1*(beta1+beta2)*(v/c(n))];
    %                
    %
    kc_radku_C2=n;                    % Kodova cisla matice C2
    kc_sloupce_C2=(2*n-1):(2*n);      % Kodova cisla matice C2
    C2(kc_radku_C2,kc_sloupce_C2)=[(beta1*beta2/2)*(v/c(n))^2   (alfa1*beta1+alfa2*beta2)*(v/c(n))];
    end
%
%

%--------------------------------------------------------------------------
%                APLIKACE  OKRAJOVYCH PODMINEK 
%               A MAZANI NULOVYCH RADKU/SLOUPCU
%                pro matice zavisle na rychlosti
%--------------------------------------------------------------------------
%
kc_OP=(OP(:,1)-1)*9+OP(:,2);  %resp. (ND@1-1)*DOF+DOF(op)
kc_OP=kc_OP';
%
%
% ****** Nalezeni Nulovych sloupcu ****************************************
Ks_abs=abs(Ks);               % Vytvorim matici tuhosti z absolutnich hodnot aby se mi nektere sloupce pri scitani nevynulovaly
SUMA=sum(Ks_abs,1);            % Soucet hodnot matice Kgl v kazdem sloupci
Nulove_sloupce=find(SUMA==0);   % Vrati kodova cisla radku ktere jsou nulove
% ****** Sestaveni vektoru OP resp. kodovych cisel  **************
Vektor_OP=[kc_OP Nulove_sloupce];
%
%
% ****** APLIKACE Vektoru_OP na [Ka] [Ta] *********************************
R=rad_modelu; %radky Kgl
S=R;            %sloupce Kgl, matice je ctvercova vzdy
x=1;
y=1;
%
for u=1:R
    if u~=Vektor_OP
        y=1;      
        for p=1:S
        if p~=Vektor_OP 
            Ka_op(x,y)=Ka(u,p);  %Na aerodynamickou matici tuhosti
            Ta_op(x,y)=Ta(u,p);  %Na aerodynamickou matici tlumeni
            y=y+1;
        end
       end
      x=x+1;
    end
end
%
%

% ****** APLIKACE Vektoru_OP na [B2] Matici ******************************
R=radky_A2;    %radky B2
S=sloupce_B2;   %sloupce B2
x=1;
y=1;
for u=1:R
      y=1;      
        for p=1:S
        if p~=Vektor_OP_Kz_radky   % Totiz radky Kz k vyskrtnuti shodne se sloupci B2 k vyskrtnuti 
            B2_op(x,y)=B2(u,p);    % Na matici B2
            %
            y=y+1;
        end
       end
      x=x+1;
    
end
%
%
%
% ****** APLIKACE Vektoru_OP na [D2] Matici ******************************
R=radky_C2;    %radky D2       
S=sloupce_B2;   %sloupce D2
x=1;
y=1;
for u=1:R
      y=1;      
        for p=1:S
        if p~=Vektor_OP_Kz_radky   % Totiz radky Kz k vyskrtnuti shodne se sloupci D2 k vyskrtnuti 
            D2_op(x,y)=D2(u,p);    % Na matici D2
            %
            y=y+1;
        end
       end
      x=x+1;
    
end
%
%
%
% ****** APLIKACE Vektoru_OP na [K3] *************************************
R=rad_modelu;           %radky K3
S=radky_C2;             %sloupce K3  shodne s radky C2
x=1;
y=1;
%
for u=1:R
    if u~=Vektor_OP
        y=1;      
        for p=1:S
             K3_op(x,y)=K3(u,p);  %Na matici K3
             %
             y=y+1;
        end
      x=x+1;
    end
end
%
%
%
%%
%--------------------------------------------------------------------------
%                         MODEL SYSTEMU
%--------------------------------------------------------------------------
%
%   u=0         u1=y3+u  |--------|    y1
%  ----------O-----------|  SM1   |--------.---------------- y
%           +|           |--------|        |
%            |y3       Strukturalni m.     |
%            /\         + stac. aero.      /\
% Aerod.    /  \                          /  \
%  matice  / K3 \                        / Kz \ Redukcni
%          ------                        ------    matice
%            |                             |            
%            |   y2      |--------|   u2   |
%            |-----------|  SM2   |--------|
%                        |--------|
%                          Duhamel
%
%
rad_modelu_op=size(Ks_op);
ak=3.2507e-05;
am=-0.0159; 
Ts_op=ak*Ks_op+am*Ms_op;
M=Ms_op-Ma_op;
T=Ts_op-Ta_op;
K=Ks_op-Ka_op;
%
% SM1
A1=[zeros(rad_modelu_op)   eye(rad_modelu_op);
    -M\K                   -M\T];
B1=[zeros(rad_modelu_op);  inv(M)];
C1=[eye(rad_modelu_op)     zeros(rad_modelu_op);
    zeros(rad_modelu_op)   eye(rad_modelu_op);
    -M\K                   -M\T];
D1=[zeros(rad_modelu_op);
    zeros(rad_modelu_op);
    inv(M)];
%
%
SM1=ss(A1,B1,C1,D1);
SM2=ss(A2,B2_op,C2,D2_op);
SYSTEM=feedback(SM1,K3_op*SM2*Kz_op,+1);
disp('Cislo podminenosti SYSTEM.a')
cond(SYSTEM.a)
[VL_VEKTOR_vypocet,LAMBDA_vypocet]=eig(SYSTEM.a);
%--------------------------------------------------------------------------
%             EXTRAHOVANI VLASTNICH CISEL + VTK  Z DANEHO ROZSAHU
%--------------------------------------------------------------------------
x=1;
y=1;
% Tento cyklus vyseparuje vl. cisl s KLADNOU Imag. casi
for n=1:rank(LAMBDA_vypocet)
     if imag(LAMBDA_vypocet(n,n))>=omega_min && imag(LAMBDA_vypocet(n,n))<=omega_max
        LAMBDA_pracovni(x,x)=LAMBDA_vypocet(n,n);
        VL_VEKTOR_pracovni(:,x)=VL_VEKTOR_vypocet(:,n);
        x=x+1;
     end
    
end

%--------------------------------------------------------------------------
%                         TRIDENI VL. CISEL
%--------------------------------------------------------------------------


[radek, sloupce]=size(LAMBDA_pracovni);
pocet_prvku_vektoru=numel(VL_VEKTOR_pracovni(:,1));

if trideni_MSP==1


MSP=zeros(radek);
VL_VEKTOR_NEW=VL_VEKTOR_pracovni;

if pocitadlo==1 
VL_VEKTOR_OLD=VL_VEKTOR_pracovni;
else
NEDELEJ_NIC=6;
end

for k=1:radek
    for l=1:sloupce
       
S1=sum(real(VL_VEKTOR_OLD(:,k)).*real(VL_VEKTOR_NEW(:,l))+imag(VL_VEKTOR_OLD(:,k)).*imag(VL_VEKTOR_NEW(:,l)));
S2=sum(real(VL_VEKTOR_OLD(:,k)).*imag(VL_VEKTOR_NEW(:,l))-imag(VL_VEKTOR_OLD(:,k)).*real(VL_VEKTOR_NEW(:,l)));
       
S3=norm(VL_VEKTOR_OLD(:,k),1)^2;
S4=norm(VL_VEKTOR_NEW(:,l),1)^2;
MSP(k,l)=sqrt(S1^2+S2^2)/sqrt(S3*S4);
    end
end


LAMBDA_pracovni_s=zeros(radek,1);
%Hledani maxi. elemntu v MSP
while max(max(MSP))~=0
  POZICE_MAX=[0 0];
  MAX=max(max(MSP));
    for k=1:radek
        for l=1:sloupce
             if MSP(k,l)==MAX
             POZICE_MAX=[k l];
             LAMBDA_pracovni_s(k,1)=LAMBDA_pracovni(l,l);
             end
        end
    end

  MSP(POZICE_MAX(1),:)=0;              %Vymaze k-ty radek matice MSP
  MSP(:,POZICE_MAX(2))=0;              %Vymaze l-ty radek matice MSP

end

else
 LAMBDA_pracovni_s=diag(LAMBDA_pracovni);   
    
end
%
%
LAMBDA(pocitadlo,:)=LAMBDA_pracovni_s(:,1);
end

%%

%--------------------------------------------------------------------------
%            POCATECNI PORMINKY PRO SIMULINK
%--------------------------------------------------------------------------
PP_SM1_Polohy=zeros(1,pocet_nodu*9);
PP_SM2=zeros(1,length(A2));
for n=1:numel(PP_kc)
      PP_SM1_Polohy(1,PP_kc(n))=IMPULS(n);
      
end
PP_SM1_Polohy(:,Vektor_OP)=[];
PP_SM1_Rychlost=zeros(1,numel(PP_SM1_Polohy));
PP_SM1=[PP_SM1_Polohy, PP_SM1_Rychlost];

%--------------------------------------------------------------------------
%            NULOVY VSTUP DO SYSTEMU
%-------------------------------------------------------------------------- 
u=zeros(1,length(A1)/2);
%
%
%--------------------------------------------------------------------------
%                  ROOT LOCUS PLOT
%-------------------------------------------------------------------------- 

if Rlocus==1    
figure(1)
plot(real(LAMBDA),imag(LAMBDA),'.k');
hold on
plot(real(LAMBDA(end,:)),imag(LAMBDA(end,:)),'or');
hold on
plot(real(LAMBDA(1,:)),imag(LAMBDA(1,:)),'xr');
sgrid
ylim([0,600])
xlim([-50,20])
sgrid
title('Root Locus - System')
end

%--------------------------------------------------------------------------
%                  Extrahovani dat pro v,g,f z LAMBDA
%-------------------------------------------------------------------------- 
    

VELIKOST=size(LAMBDA);
FREKVENCE=(zeros(VELIKOST));
TLUMENI=(zeros(VELIKOST));

for R=1:VELIKOST(1)
    for S=1:VELIKOST(2)
        FREKVENCE(R,S)=imag(LAMBDA(R,S))/2/pi;
        TLUMENI(R,S)=real(LAMBDA(R,S))/imag(LAMBDA(R,S));
    end
end


%--------------------------------------------------------------------------
%                  v-g-f diagramy
%-------------------------------------------------------------------------- 


if vgf>0
figure(3)
subplot(2,1,1)
plot(V,TLUMENI)%,'.k')
xlabel('Rychlost letu [m/s]')
ylabel('Tlumeni [-]')
%ylim([-0.06,0.015])
grid
subplot(2,1,2)
plot(V,FREKVENCE)%,'.k')
xlabel('Rychlost letu [m/s]')
ylabel('Frekvence [Hz]')
grid

figure(4)
subplot(2,1,1)
plot(V,TLUMENI,'.k')
xlabel('Rychlost letu [m/s]')
ylabel('Tlumeni [-]')
%ylim([-0.06,0.015])
grid
subplot(2,1,2)
plot(V,FREKVENCE,'.k')
xlabel('Rychlost letu [m/s]')
ylabel('Frekvence [Hz]')
grid
end




%--------------------------------------------------------------------------
%                  HLEDANI RYCHLOSTI FLUTTERU
%-------------------------------------------------------------------------- 

%FREKVENCE a TLUMENI --> Mody jsou ve sloupcich, Radek znaci rychlost letu
x=1;
for R=2:VELIKOST(1)  %RYCHLOST LETU
    
for S=1:VELIKOST(2)  %CISLO MODU
 
    delta_f=abs((FREKVENCE(R-1,S)-FREKVENCE(R,S)))/FREKVENCE(R-1,S);
    
    if TLUMENI(R,S)>0 && TLUMENI(R-1,S)<0 && delta_f<0.02
                  
        v1=V(R-1);
        v2=V(R);
        g0=0;
        f1=FREKVENCE(R-1,S);
        f2=FREKVENCE(R,S);
        g1=TLUMENI(R-1,S);
        g2=TLUMENI(R,S);
       
        
        VFL(x,1)=v1+((v2-v1)*((g0-g1)/(g2-g1))); 
        FFL(x,1)=f1+((f2-f1)*((VFL(x,1)-v1)/(v2-v1)));
        NIC=string("-");
        INFO(x,1)=NIC;
        x=x+1;
        
    elseif TLUMENI(R,S)>0 && TLUMENI(R-1,S)<0  
        
        v1=V(R-1);
        v2=V(R);
        g0=0;
        f2=FREKVENCE(R,S);
        %
        delta_f2=abs(FREKVENCE(R-1,:)-f2)./FREKVENCE(R-1,:); % Procentualni vyjadreni odchylky f2 v predchozim radku
        kc_f1=find(delta_f2==min(delta_f2));
        f1=FREKVENCE(R-1,kc_f1);  %NALEZT dle frekvnece
        %
        g1=TLUMENI(R-1,kc_f1);  %NALEZT dle frekvence
        g2=TLUMENI(R,S);
        
            if g1<0
            VFL(x,1)=v1+((v2-v1)*((g0-g1)/(g2-g1))); 
            FFL(x,1)=f1+((f2-f1)*((VFL(x,1)-v1)/(v2-v1)));
            PROHOZ=string("Prohazovani modu");
            INFO(x,1)=PROHOZ;
            x=x+1;
            end
          
            elseif TLUMENI(R,S)==0
            VFL(x,1)=V(R); 
            FFL(x,1)=FREKVENCE(R,S);
            INFO(x,1)=NIC;
            x=x+1;
      
    end
end

end

kc=find(TLUMENI(1,:)>0);
if kc~=0
VFL(x,1)=0;
FFL(x,1)=FREKVENCE(1,kc(1));
POZOR=string("Na zacatku nestabilni mod");
INFO(x,1)=POZOR;

end

% Tisk vysledku
Vysledky(:,3)=INFO;
Vysledky(:,1)=round(VFL,1);
Vysledky(:,2)=round(FFL,1);

Vysledky
disp('V_fl[m/s]    F_fl[Hz]    PROHOZ')
disp('***********')
disp('KONEC')