%% Hilbert-Algo-Test



pkg load signal


% Hilbert Trafo für Controller-Implementation nach: https://www.mikrocontroller.net/topic/480404#7103779
% sample_in : das datensignal links am Eingang
% delay_out : Der aktuelle Ausgang des Verzögerers (das kann nicht hier in der Funktion gemacht werden)
% coeff     : Der Koeffizient
function [sample_out,delay_in]=allpass_calc(sample_in,delay_out,coeff)

  signal_top_right = coeff * ( delay_out - sample_in );
  delay_in         = signal_top_right + sample_in;
  sample_out       = signal_top_right + delay_out;
  
endfunction


% Hilbert Trafo für Controller-Implementation nach: https://www.mikrocontroller.net/topic/480404#7103779
% mit reellen Koeffizienten (wie im Bild) 

function sample_z=hilbert_algo_1(sample_in)

  % delay storage
  persistent i1=0;           %  inphase Z-1 
  persistent i2a=0;          %  inphase 0.3900 block
  persistent i2b=0;          %  inphase 0.3900 block
  persistent i3a=0;          %  inphase 0.8906 block
  persistent i3b=0;          %  inphase 0.8906 block

  persistent q1a=0;          %  quadratur 0.1206 block
  persistent q1b=0;          %  quadratur 0.1206 block
  persistent q2a=0;          %  quadratur 0.6628 block
  persistent q2b=0;          %  quadratur 0.6628 block

  % inphase
  
  % der inphase 0.3900 Teil
  [i2_out,delay_in]=allpass_calc(i1,i2b,0.3900);
  i2b=i2a;
  i2a=delay_in; 
  % der inphase 0.8906 Teil
  [sample_out_i,delay_in]=allpass_calc(i2_out,i3b,0.8906);
  i3b=i3a;
  i3a=delay_in; 
  % der inphase Z-1 Teil
  i1=sample_in;
  
  % quadratur  

  % der quadratur 0.1206 Teil
  [q1_out,delay_in]=allpass_calc(sample_in,q1b,0.1206);
  q1b=q1a;
  q1a=delay_in; 

  % der quadratur 0.6628 Teil
  [sample_out_q,delay_in]=allpass_calc(q1_out,q2b,0.6628);
  q2b=q2a;
  q2a=delay_in; 
  
  
  
  sample_z=sample_out_i+i*sample_out_q;
  
endfunction




% Hilbert Trafo für Controller-Implementation nach: https://www.mikrocontroller.net/topic/480404#7103779
% Koeffizienten nach diesen Quellen:
% https://www.mikrocontroller.net/topic/92630#839661
% https://web.archive.org/web/20070814013543/http://yehar.com/ViewHome.pl?page=dsp/hilbert/011729.html

function sample_z=hilbert_algo_2(sample_in)

  % delay storage
  persistent i0=0;           %  inphase Z-1 
  persistent i1a=0;          %  inphase 1. block
  persistent i1b=0;          %  
  persistent i2a=0;          %  inphase 2. block
  persistent i2b=0;          %  
  persistent i3a=0;          %  inphase 3. block
  persistent i3b=0;          %  
  persistent i4a=0;          %  inphase 4. block
  persistent i4b=0;          %  

  persistent q1a=0;          %  quadratur 1. block
  persistent q1b=0;          %  
  persistent q2a=0;          %  quadratur 2. block
  persistent q2b=0;          %  
  persistent q3a=0;          %  quadratur 3. block
  persistent q3b=0;          %  
  persistent q4a=0;          %  quadratur 4. block
  persistent q4b=0;          %  

  % inphase
  
  % inphase 1. Block
  [i1_out,delay_in]=allpass_calc(i0,i1b,0.6923877778065*0.6923877778065);
  i1b=i1a;
  i1a=delay_in; 
  % inphase 2. Block
  [i2_out,delay_in]=allpass_calc(i1_out,i2b,0.9360654322959*0.9360654322959);
  i2b=i2a;
  i2a=delay_in; 
  % inphase 3. Block
  [i3_out,delay_in]=allpass_calc(i2_out,i3b,0.9882295226860*0.9882295226860);
  i3b=i3a;
  i3a=delay_in; 
  % inphase 4. Block
  [sample_out_i,delay_in]=allpass_calc(i3_out,i4b,0.9987488452737*0.9987488452737);
  i4b=i4a;
  i4a=delay_in; 
  % der inphase Z-1 Teil
  i0=sample_in;
  
  % quadratur  

  % quadratur 1.Block
  [q1_out,delay_in]=allpass_calc(sample_in,q1b,0.4021921162426*0.4021921162426);
  q1b=q1a;
  q1a=delay_in; 
  % quadratur 2.Block
  [q2_out,delay_in]=allpass_calc(q1_out,q2b,0.8561710882420*0.8561710882420);
  q2b=q2a;
  q2a=delay_in; 
  % quadratur 3.Block
  [q3_out,delay_in]=allpass_calc(q2_out,q3b,0.9722909545651*0.9722909545651);
  q3b=q3a;
  q3a=delay_in; 

  % quadratur 4. Block
  [sample_out_q,delay_in]=allpass_calc(q3_out,q4b,0.9952884791278*0.9952884791278);
  q4b=q4a;
  q4a=delay_in; 
  
  
  
  sample_z=sample_out_i+i*sample_out_q;
  
endfunction







function sample_z=allpass_alleine(sample_in)

  % delay storage
  persistent i1=0;           %  inphase Z-1 
  persistent q1a=0;          %  quadratur 0.1206 block
  persistent q1b=0;          %  quadratur 0.1206 block


 
  % quadratur  

  % der quadratur 0.1206 Teil
  [q1_out,delay_in]=block2_calc(sample_in,q1b,0.1206);

   [sample_in delay_in q1a q1b q1_out]    % debug output

  q1b=q1a;
  q1a=delay_in; 
 
  
  
  sample_out_i=q1_out;
  sample_out_q=0;
  
  sample_z=sample_out_i+i*sample_out_q;
 


endfunction




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                                                                                         %%%
%%%                                               Main                                                      %%%
%%%                                                                                                         %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




%filename='wav/zahlen1_8kHz.wav';  
filename='wav/zahlen1_16kHz.wav';  
[data,samplerate]=audioread(filename);
data=data';             % Spaltenvektor draus machen
%data=data(1:30000);     % Ausschnitt zum testen ---------------------------------------------------------------
%data=data(4800:4810);     % Ausschnitt zum testen ---
max_amplitude=max(abs(data));   % Amplitude auf 1 normieren
data=data/max_amplitude;
    
datalen=length(data);
n=1:datalen;
freq_axis=0.001*samplerate*n/datalen;
freq_axis=freq_axis-0.001*samplerate/2;   % Frequenzachse in kHz



% eingebaute Hilbert-Trafo

data_z_intern=hilbert(data);
fft_data_z_intern=fft(data_z_intern);


%% eigene Hilbert-Trafo berechnen

data_z=zeros(1,datalen);

for n=1:datalen,   %  im Streaming-Stil wie es im controller laufen würde

  %data_z(n)=hilbert_algo_1(data(n));   % 4pol IIR
  data_z(n)=hilbert_algo_2(data(n));    % 8pol IIR
  %data_z(n)=allpass_alleine(data(n));
  
  if bitand(n,65535)==0   % Fortschrittsanzeige
    n
  endif

end;

data_z=conj(data_z);    % negative Frequenzen werden zu Null, statt der Positiven

max_amplitude=max(abs(data_z));   % Amplitude auf 1 normieren
data_z=-data_z/max_amplitude;

fft_data_z=fft(data_z);
fft_data  =fft(data  );


%%%%%%%%%%%%%%%%%%%%%% Anzeige

figure(100)  % XY-Daten nach der Hilbert-Trafo
plot(1:datalen,data,1:datalen,data_z)


figure(101)
semilogy(freq_axis,abs(fftshift(fft_data_z))+1e-6,freq_axis,abs(fftshift(fft_data_z_intern))+1e-6)  % verschiedene Hilbert-Algo vergleichen
xlabel('Freq [kHz]');
title('FFT der Hilbert-Trafo')

%figure(102)
%plot(freq_axis,unwrap(arg(fftshift(fft_data_z)))-unwrap(arg(fftshift(fft_data))))  % % Phase vergleichen
%plot(freq_axis,arg(fftshift(fft_data_z))-arg(fftshift(fft_data)))  % % Phase vergleichen
%plot(freq_axis,180/pi*(arg(fftshift(fft_data_z))/arg(fftshift(fft_data_z_intern))))  % Phase vergleichen
%xlabel('Freq [kHz]');
%title('Phasendifferenz')

figure(103)
semilogy(freq_axis,abs(fftshift(fft_data))+1e-6,freq_axis,abs(fftshift(fft_data_z))+1e-6)  % verschiedene Hilbert-Algo vergleichen
#plot(freq_axis,180/pi*(arg(fftshift(fft_data_z))/arg(fftshift(fft_data_z_intern))))  % Phase vergleichen
%xlabel('Freq [kHz]');
title('FFT orig signal / 8pol IIR Hilbert Algo / 16kHz Sample Rate')
xlabel('Freq [kHz]');
grid on