%skript pro asociaci zpráv DF17 a pro výpoèet polohy pomocí MLAT metody
%ve 2D

close all
clear all

format long g

%Vytvoreni TCP/IP objektu 't', urceni ip adresy a portu
 t = tcpip('127.0.0.1', 10000);
 
  
 ip_1=14732121136;  %147.32.121.136 Strahov
 ip_2=147327384; %147.32.73.84      Horská
 ip_3=1931652183; %193.165.218.3  Pankrác
 

% Nastaveni velikosti vyrovnavaci pameti 
 set(t, 'InputBufferSize', 32768);

 
% Pripojeni na pozadovany server 
fopen(t); 

%==============================
filename = 'testdata16.xlsx';

n=500000;         %poèet pøijatých zpráv
    
rozptyl=4;     %poèet zpráv mezi kterými se vyhledává

roz=100;        %rozptyl øazení èasových znaèek
%==============================
%pro vypsání výsledkù do xls souboru
A=cell(100000,6);
vyhovuje=0;

pocet_shod=0; %pro informaci poctu shod
 %pro ukládání zpráv (viz níže)
k=0;
L=0;

K1=cell(n/2,8); %definování pole K1 (rychlejší zpracování)

    %stopky
tic;
    %cyklus pro pøíjem zpráv
for i=1:n

%=====================================  
% %     %èasové omezení pøíjmu zpráv
% % d=toc;
% %     if d>500
% % break;
% %     end   
%====================================

 % pøeètení øádku z pøijímaèe
tline = fgetl(t);
 % pøevod na øetìzec 
char_msg = char(tline);

%nalezení @ pro rozdeleni zpravy (ruzne IP maji ruznou delku)

zavinac = find(char_msg == '@'); 
     
IP=char_msg(1:zavinac-2); %pouze IP adresa

zprava=char_msg(zavinac+1:end-2); %zpráva od @ dále

% podminka delky retezce pro dlouhe zpravy (40 znaku)
if length(zprava) == 40 

   
    %podmínky pro specifikaci DF viz odkaz
    %http://www.tech-software.net/MODE_S.htm
     %mozne pocatecni kombinace cisel pro df17:88,89,8A,8B,8C,8D,8E,8F
if  zprava(13) == '8' 
    
     hex_type = zprava(21:22);%type code
    dec_type=hex2dec(hex_type); %prevod z hex do desitkove sous.
    bin_type=dec2bin(dec_type,8); %pøevod do 48bit binarni soust
    
    type_code=bin2dec(bin_type(1:5)); % type code dle bitu
 

   %separování zpráv které nesou informaci o poloze letadla 
 if type_code >= 9 && type_code <= 18
    
    %pøiètení 1 pro zápis nové zrávy do pole
    k=k+1;
   
%zmìna IP adresy na èíslo ( napø 192.168.1.1 na 19216811)
IP_tecka=find(IP == '.');

IP_cislo=sprintf('%s%s%s%s' ,IP(1:IP_tecka(1)-1) ,IP(IP_tecka(1)+1:IP_tecka(2)-1)...
                            , IP(IP_tecka(2)+1:IP_tecka(3)-1),IP(IP_tecka(3)+1:end) );
IP_cislo=str2double(IP_cislo);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    hex_cas = zprava(1:12);%casova znacka
    dec_cas=hex2dec(hex_cas); %prevod z hex do desitkove sous.
    bin_cas=dec2bin(dec_cas,48); %pøevod do 48bit binarni soust
    

    bin_cas_sec=bin_cas(1:18); %separování èásti, která nese informaci o sekundách
    bin_cas_nano=bin_cas(19:48);%separování èásti která nese informaci o nanosekundách

    T_nano = bin2dec(bin_cas_nano); %pøevod nano do desítkové soustavy
    T_sec = bin2dec(bin_cas_sec);   %pøevod sekund do desítkové soustavy

    cas=sprintf('%d.%d',T_sec, T_nano); %spojení sekund a nanosekund
    cas=str2double(cas);  %pøevedení pøedchozího spojení na èíslo


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%zápis zpráv do pole
K1{k,1}=zprava; %obsah ADS-B zprávy z prijimace
K1{k,2}=zprava(21:34); %cista zprava DF_17 (21:34)
K1{k,3}=IP_cislo; %IP adresa prijimace

K1{k,4}=T_sec;
K1{k,5}=T_nano;

%TRIDICI ALG.
%============================================================

if k >= roz  %podmínka pro zahájení vyhledávání jakmile bude naèten urèitý poèet zpráv (roz)
   
    
    L=L+1; %pro postupné posouvání se v pùvodním poli
    
    %doèasné pole pro øazení zpráv podle èasu (zprávy z pøijímaèù nejsou
    %poskládány podle èasu
    

     %vytvoøení pole které se bude seøazovat    
    K2=K1(L:L+roz-1,1:end);
    %øazení zpráv podle èasových znaèek 
    K2=sortrows(K2,4:5);
    %pøepis seøazeného pole zpìt
    K1(L:L+roz-1,1:end)=K2;

    
    
    %vytvoøení pole pro vyhledávací cyklus
    K3=K1(L:L+rozptyl-1,2);
       
    %vyhledávací cyklus 
    %========================
    for Z1=1:rozptyl
               

   index = find(strcmp(K3,K3{Z1,1}));  %nalezení stejných zpráv a urèení místa shod

 
%podmínka pro poèet shodných zpráv + kvuli vyskytu opakujicich se zprav
% (urèení podle posledního èísla v IP)
    if   length(index) == 3 && K1{index(1)+L-1,3} ~= K1{index(2)+L-1,3}...
                            && K1{index(1)+L-1,3} ~= K1{index(3)+L-1,3}...
                            && K1{index(2)+L-1,3} ~= K1{index(3)+L-1,3}
        
               
        %urèení místa výsledku v celém poli pøijatých zpráv
        vysledek=index+L-1;
        
        %poèitadlo shodných zpráv
        pocet_shod=pocet_shod+1;
        
         %smazani vysledku pro opakovane vyhledavani stejného výsledku
        K1{vysledek(1),2}=rand;
        K1{vysledek(2),2}=rand;
        K1{vysledek(3),2}=rand;

        K3{index(1)}=rand;
        K3{index(2)}=rand;
        K3{index(3)}=rand;
   


  %pøevod èasu na èíslo    
             
    T_sec_1 = K1{vysledek(1),4};   %zápis sekund
    T_nano_1 = K1{vysledek(1),5}; %zápis nanosekund

    cas_1=sprintf('%d.%d',T_sec_1, T_nano_1); %spojení sekund a nanosekund
    cas_1=str2double(cas_1);  %pøevedení pøedchozího spojení na èíslo
    
    %====================================
    % to same jako v pøedchozim kroku jen pro druhou èas. znaèku
    
    T_sec_2 = K1{vysledek(2),4};
    T_nano_2 = K1{vysledek(2),5};

    cas_2=sprintf('%d.%d',T_sec_2, T_nano_2);
    cas_2=str2double(cas_2);
    %================================
    % to same jako v pøedchozim kroku jen pro tøetí èas. znaèku
    
    T_sec_3 = K1{vysledek(3),4};
    T_nano_3 = K1{vysledek(3),5};

    cas_3=sprintf('%d.%d',T_sec_3, T_nano_3);
    cas_3=str2double(cas_3); 
    
    
    %prevod ICAO znacky z HEX do desitkove soust.
    
    ICAO_hex_1= K1{vysledek(1),1}(15:20);
      
         
   %rozdíl èasù, ošetøení aby se èasy odeèetly vždy stejnì a ne v
   %závislosti na poøadí pøíjmu zpráv

    if K1{vysledek(1),3} == ip_1 
            
        if K1{vysledek(2),3} == ip_2
             delta_1=T_nano_2 - T_nano_1; 
             delta_2=T_nano_3 - T_nano_1;     
        else
            delta_1=T_nano_3 - T_nano_1; 
            delta_2=T_nano_2 - T_nano_1;
        end
    
    elseif K1{vysledek(2),3} == ip_1
          
        if K1{vysledek(1),3} == ip_2
             delta_1=T_nano_1 - T_nano_2; 
             delta_2=T_nano_3 - T_nano_2;     
        else
            delta_1=T_nano_3 - T_nano_2; 
            delta_2=T_nano_1 - T_nano_2;
        end
       
    elseif K1{vysledek(3),3} == ip_1
        
        if K1{vysledek(1),3} == ip_2
             delta_1=T_nano_1 - T_nano_3; 
             delta_2=T_nano_2 - T_nano_3;     
        else
            delta_1=T_nano_2 - T_nano_3; 
            delta_2=T_nano_1 - T_nano_3;
        end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
Ralt=5000; %nastaveni relativní výšky letadel (v metrech)

 %FUNKCE PRO PROPOUŠTÌNÍ POUZE ZPRÁV BLÍZKÝCH LETADEL       
    %pøesná poloha z ADS-B
 [Rlat,Rlon]=fce_DF17_TC9_22_krom19_CPR_Air_decoder(K1{vysledek(1,1)}(13:40)); %13-40 
 
 %transformace souradnic priblizneho stredu prahy (Jiraskuv most)   
WGS84_praha=[50.0757864 14.4117611 342];
ECEF_praha=WGS842ECEF(WGS84_praha);
[ee, nn, uu]=xyz2enu(WGS84_praha(1),WGS84_praha(2),WGS84_praha(3),ECEF_praha(1),ECEF_praha(2),ECEF_praha(3));
ENU_praha=[ee,nn,uu];
%transformace polohy letounu
WGS84_acft=[Rlat Rlon Ralt ];
ECEF_acft=WGS842ECEF(WGS84_acft);
[ee, nn, uu]=xyz2enu(WGS84_praha(1),WGS84_praha(2),WGS84_praha(3),ECEF_acft(1),ECEF_acft(2),ECEF_acft(3));
ENU_acft=[ee, nn, uu];

%vzdálenost stredu prahy od letounu
hranice= abs(((ENU_praha(1)-ENU_acft(1))^2)+(ENU_praha(2)-ENU_acft(2))^2)^(1/2);       
 
%podmínka pro vzdálenost letounu od prahy
if hranice < 40000
vyhovuje=vyhovuje+1;

%funkce pro poèítání 2D polohy multilateraèní metodou
     mlt_poloha=poloha_mlat_2(delta_1,delta_2,Rlat,Rlon,Ralt);
    

  
     %zobrazení vypoètené odchylky
     vysledek=sprintf('%d %d %d', vysledek(1),vysledek(2),vysledek(3)); %pouze pro možnost vypsání matice do jednoho øádku
    
     if abs(mlt_poloha(1))<= abs(mlt_poloha(2))
     
         odchylka=mlt_poloha(1);
     else
         odchylka=mlt_poloha(2);
     end
          fprintf(' %s , Odchylka %.f  Vzdálenost %.f    \n',ICAO_hex_1, odchylka, hranice);   
   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

A{vyhovuje,1}=odchylka;
A{vyhovuje,2}=hranice;
A{vyhovuje,3}=ICAO_hex_1;
A{vyhovuje,4}=Rlat;
A{vyhovuje,5}=Rlon;
A{vyhovuje,6}=mlt_poloha(1);
A{vyhovuje,7}=mlt_poloha(2);
end
    end
    end  
end
end
end
end
end

%zastavení poèítání
c=toc;

%podmínka pro to èím je zastaven algoritmus (cas nebo pocet zprav)
if i>n
fprintf('Pøíjem trval %f sec., poèet zpráv = %d, poèet shod = %d, %d výpoètù, %f msg/sec.\n', c, n,pocet_shod,vyhovuje ,n/c);

else
fprintf('Pøíjem trval %f sec., poèet zpráv = %d, poèet shod = %d, %d výpoètù, %f msg/sec.\n', c, i,pocet_shod,vyhovuje ,i/c);
end


xlswrite(filename,A);

% Odpojení od serveru 
fclose(t); 


