% Frequency LMS Adaptive Filter -FLMS- (DEBUG)
% --------------------------------------------
%
% Aufruf:
% Function [ys,ee,wf,fpo]=flms_d(x,d,alpha,N,C,L,wi)
%
% Parameter:
% x    : Eingangsvektor
% d    : Referenzvektor
% alpha: konstante Schrittweite 0 < alpha <= 1.0
% N    : Anzahl Filterkoeffizienten 
% C    : FFT-Länge, idealerweise eine Zweier-Potenz (0=Auto)
% L    : Länge eines Verarbeitungsblocks (idealerweise C=L+N=2N)
% wi   : Startwerte der Filterkoeffizienten im Zeitbereich
%
% Rückgabewerte:
% ee   : Adaptitionsfehler
% ys   : Filterausgang
% wf   : Filterkoeffizienten am Ende der Adaption
% fpo  : Anzahl benötigten der Floating-Point Operationen
%
% Zu Debug-Zwecken werden folgende Variablen als Binär-Dateien gespeichert:
% wf.dat : Filterkoeffizienten am Ende der Adaption (Länge N)

% -------------------------------------------------------------------------
% Datum : 20.7.2001
% Autor : Jens Ahrensfeld
% Thema : Diplomarbeit
% Datei : flms_d.m
% Benötigte Dateien: dspfft.m, dspifft.m
%
% -------------------------------------------------------------------------
function [ys,ee,wf,fpo]=flms_d(x,d,alpha,N,C,L,wi)

% -------------------------------------------------------------------------
% Überprüfen der Parameter 
% -------------------------------------------------------------------------
if (N ~= fix(N)),
    error('Fehler: N muss eine ganze Zahl sein!');
end;

if (C ~= fix(C)),
    error('Fehler: C muss eine ganze Zahl sein!');
end;

if (L ~= fix(L)),
    error('Fehler: L muss eine ganze Zahl sein!');
end;

% -------------------------------------------------------------------------
% Automatik-Auswahl
% -------------------------------------------------------------------------
% Falls C=0 => Automatische Wahl von C=2^r
% Anahme: C = L + N 
if (C==0)
  C = 2^ceil(log2(L+N-1))
end;

% -------------------------------------------------------------------------
% FLMS Parameter
% -------------------------------------------------------------------------
gamma  = 1.0/C;               % Sicherheitskonstante vermeidet Division durch Null
lambda = 0.6;                 % Vergessensfaktor

% -------------------------------------------------------------------------
% Initialisierungen
% -------------------------------------------------------------------------
Lx = length(x);              % Länge des Eingangsvektors
K = Lx/L;                    % Anzahl Verarbeitungsblöcke

ee=zeros(Lx,1);              % Fehlervektor
ys=zeros(Lx,1);              % Filterausgang, Schaetzung von d
wf = zeros(C,1);             % Filter im Zeitbereich
PX = zeros(C,1);             % Schaetzung des  Leistungsdichtespektrums

WS = zeros(C,1);             % Filterkoeff. im Frequenzbereich
X = zeros(C,1);              % Eingangsvektor im Frequenzbereich
mu = zeros(C,1);             % Variable Schrittweite
yy = zeros(C,1);             % Filterausgang im Zeitbereich
YS  = zeros(C,1);            % Filterausgang im Frequenzbereich
xzp = zeros(Lx+C-L,1);       % Eingangsvektor mit C-L führenden Nullen

xzp(C-L+1:Lx+C-L) = x;       % Einfügen von Np führenden Nullen an den Eingangsvektor

% Anfangswerte der Filterkoeffienten in den Frequenzbereich transformieren
%WS(1:C) = dspfft(wi(1:N),C);

% -------------------------------------------------------------------------
% Algorithmus Start
% -------------------------------------------------------------------------
flops(0);
for k=1:K,

  kL = (k-1)*L;

  % Transformation des aktuellen Eingangsvektors in den Frequenzbereich
  X(1:C) = dspfft(xzp(kL+1:kL+C),C); 

  % Schnelle FFT-Faltung
  YS = WS(1:C).*X(1:C) *C;

  % Transformation des Filterausgangs in den Zeitbereich
  yy = real(dspifft(YS,C));
  
  % Abspeichern der letzten L Werte des Filterausgangs
  ys(kL+1:kL+L) = yy(C-L+1:C);

  % Berechnung des Fehlers e(k)
  ee(kL+1:kL+L) = d(kL+1:kL+L) - ys(kL+1:kL+L);
  
  % Transformation des Fehlers in den Frequenzbereich
  E  = dspfft([zeros(C-L,1); ee(kL+1:kL+L)],C);

  % Schätzung der mittleren Eingangsleistung Px(k) aus x(k)
  PX = abs((1-lambda)*conj(X(1:C)).*X(1:C) *C + lambda*PX);

  % Berechnung der variablen Schrittweite mu(k)
  % mu(k) wird niemals grösser als eins
  mu(1:C) = (alpha*gamma) ./(PX+gamma);

  % Filterkoeffizenten-Update
  WS(1:C) = WS(1:C) + mu(1:C) .* conj(X(1:C)) .* E *C;


% Projektion der Filterkoeffizienten 
  wf(1:C)  = real(dspifft(WS(1:C),C));
  WS(1:C) = dspfft(wf(1:N),C);

end;

wf = [wf(1:C-L+1) ; zeros(L-1,1)];   % Filterkoeffizienten am Ende der Adaption

% Speichern der Koeffizienten in Datei (Genauigkeit Float32)
fid = fopen('wf.dat','wb');
fwrite(fid,wf,'float32');
fclose(fid);

fpo=flops;

% -------------------------------------------------------------------------
% Ende FLMS_D.M 
% -------------------------------------------------------------------------