function [B,f1]=pfd(x_data,fs,plotflag,f1)
% PFD Inharmonicity and F0 estimation algorithm.
% 
% Description:
%     This is an algorithm for estimating F0 and
%     inharmonicity from, especially, piano tones. For more details,
%     see:
%     
%     J. Rauhala, H.-M. Lehtonen, and V. Välimäki, “Fast automatic 
%     inharmonicity estimation algorithm,” J. Acoust. Soc. Am., vol. 
%     121, no. 5, pp. EL184-EL189, 2007.
%     
%     J. Rauhala and V. Välimäki, “F0 estimation of inharmonic piano 
%     tones using partial frequencies deviation method,” accepted for 
%     publishing in International Computer Music Conference, Copenhagen, 
%     Denmark, 2007.
%     
% Parameters:
% 
%     x_data      input signal
%     fs          sampling frequency
%     f1          (optional) fundamental frequency in case when external
%                 F0 estimation is used for the preliminary F0 estimation
%     plotflag    (optional) if set to 1, then the script plots the final
%                 PFD curve for evaluating the quality of the estimation
%                 
% Example:
% 
%     x = wavread('mytone.wav');
%     [B,f1]=pfd(x,44100,1);
%     
%     
% Created by Jukka Rauhala 29.06.2007
% Copyright (c) Jukka Rauhala 2007
%     

if nargin<3
    plotflag = 0;
end

if nargin <4
   f1 = getf1(x_data); 
end

x_data=x_data(:);

start = find_start(x_data);
len = start+round(fs);
if len>length(x_data)
    len = length(x_data);
end

x_data_win = x_data(1+start:len)-mean(x_data(1+start:len));
clear x_data;

x_data_win = x_data_win.*blackman(length(x_data_win));

Xfft = abs(fft(x_data_win,2^16));
fftlen = ceil(length(Xfft)/2);
scale = 0.5*fs/fftlen;
Xfft=Xfft(1:fftlen);

fk = ([1:fftlen]-1).*scale;
amps = Xfft;


winlen = round(f1*5);

maxk = 50;

q = ceil(20000/winlen);
if q > ceil(maxk/5)
    q = ceil(maxk/5);
end
amps = 20.*log10(amps);
ti = [];
for i = 1:q
    fi = find(fk>(i-1)*winlen & fk<=i*winlen);
    piz = find_all_peaks(amps(fi)',1);
    [p,pi] = findmaxs(amps(fi(piz)),10);
    ti = [ti fi(piz(pi))];
end
ti = sort(ti);


Bm = 1;
Best = 0.0001;
kx = 2:maxk;
f1d = f1*0.4;

i = 0;

while i<40 & (i==0 | abs(Bm)>10^-4)
    i = i+1;
    trend = [];

    fz_est = kx.*f1.*sqrt(1+Best.*kx.*kx)./sqrt(1+Best);
    fz_est = round(fz_est./scale)*scale;
    for k=2:maxk
        fz_frame_min = fz_est(k-1)-f1d;
        fz_frame_max = fz_est(k-1)+f1d;

        fi2 = find(fk(ti)>fz_frame_min & fk(ti)<=fz_frame_max);

        [m,mi] = max(amps(ti(fi2)));

        trend = [trend (ti(fi2(mi))-1)*scale-fz_est(k-1)];
    end

    rx = sum(sign(diff(trend)));

    if rx(1)<0
        if Bm>=0
            Bm = -Bm/2;
        end
    else
        if Bm<=0
            Bm = -Bm/2;
        end
    end

    Best = Best*10^Bm;
end


fm = 0.005;
fsign = 0;

i = 0;

while i<100 & (i==0 | fm>10^-5)
    i = i+1;
    trend = [];

    fz_est = kx.*f1.*sqrt(1+Best.*kx.*kx)./sqrt(1+Best);
    fz_est = round(fz_est./scale)*scale;
    for k=2:maxk
        fz_frame_min = fz_est(k-1)-f1d;
        fz_frame_max = fz_est(k-1)+f1d;

        fi2 = find(fk(ti)>fz_frame_min & fk(ti)<=fz_frame_max);

        [m,mi] = max(amps(ti(fi2)));

        trend = [trend (ti(fi2(mi))-1)*scale-fz_est(k-1)];
    end



    qs = sign(mean(trend(1:ceil(length(trend)/2))));
    if fsign==0
        fsign = qs;
    else
        if fsign==qs
        else
            fm = fm/2;
            fsign = qs;

        end
    end
    f1 = f1*(1+fm*fsign);
end

Bm = 1;
i = 0;

while i<40 & (i==0 | abs(Bm)>10^-4)
    i = i+1;
    trend = [];

    fz_est = kx.*f1.*sqrt(1+Best.*kx.*kx)./sqrt(1+Best);
    fz_est = round(fz_est./scale)*scale;
    for k=2:maxk
        fz_frame_min = fz_est(k-1)-f1d;
        fz_frame_max = fz_est(k-1)+f1d;

        fi2 = find(fk(ti)>fz_frame_min & fk(ti)<=fz_frame_max);

        [m,mi] = max(amps(ti(fi2)));

        trend = [trend (ti(fi2(mi))-1)*scale-fz_est(k-1)];
    end


    rx = sum(sign(diff(trend)));

    if rx(1)<0
        if Bm>=0
            Bm = -Bm/2;
        end
    else
        if Bm<=0
            Bm = -Bm/2;
        end
    end

    Best = Best*10^Bm;
end




B = Best;

fm = 0.005;
fsign = 0;
i = 0;

while i<100 & (i==0 | fm>10^-5)
    i = i+1;
    trend = [];

    fz_est = kx.*f1.*sqrt(1+Best.*kx.*kx)./sqrt(1+Best);
    fz_est = round(fz_est./scale)*scale;
    for k=2:maxk
        fz_frame_min = fz_est(k-1)-f1d;
        fz_frame_max = fz_est(k-1)+f1d;

        fi2 = find(fk(ti)>fz_frame_min & fk(ti)<=fz_frame_max);

        [m,mi] = max(amps(ti(fi2)));

        trend = [trend (ti(fi2(mi))-1)*scale-fz_est(k-1)];
    end

    qs = sign(mean(trend(1:ceil(length(trend)/2))));
    if fsign==0
        fsign = qs;
    else
        if fsign==qs
        else
            fm = fm/2;
            fsign = qs;

        end
    end
    f1 = f1*(1+fm*fsign);
end

if plotflag == 1
    plot(trend,'bo-');
end



% %--
function start=find_start(x)
limit = 0.01;

[maxamp,n] = max(abs(x));
[maxamp2,n2] = max(abs(x(1:(n-100))));
while maxamp2>limit*maxamp & n > 1,
    n = n2-1;
    [maxamp2,n2] = max(abs(x(1:(n-100))));

end
start = n;


% ---
function [peakv,peaki] = findmaxs(data,n)
ampq = data;
peakv = [];
peaki = [];
for i=1:n
    [m,mi] = max(ampq);
    peakv = [peakv m];
    peaki = [peaki mi];
    ampq(mi) =min(ampq)-1;
end

%--
function indices=find_all_peaks(data,maxmin)
% maxmin = 1 if maximum, -1 if minimum
if isempty(data)==1
    indices = [];
else
    it = sign(diff(data));
    indices = [find(diff(it)==-2*sign(maxmin))]+1;
    if it(1)==-maxmin
        indices = [1 indices];
    end
    indices = [indices length(data)];
end

%--
function f1=getf1(x_data)
oct = determine_octave(x_data);
Xfft = abs(fft(x_data,2^20));
fftlen = length(Xfft)/2;
fs = 44100;
scale = 0.5*fs/fftlen;
    start = ceil(oct*0.8/scale);
Xfft=Xfft(start:ceil(oct*1.25/scale));

[m,mi]=max(Xfft);
if mi>1 && mi < length(Xfft)
    [interpolated_maximum,offset]=peakint(Xfft(mi-1:mi+1));
else
    offset = 0;
end
f1 = (mi+offset+start-1)*scale;
    



%--
function res = rec_find_oct(Xfftz,iz,fmax,fmin,lim)

fi = find(iz<fmax & iz>fmin);

[m,mi] = max(Xfftz(iz(fi)));

res = [];

mi = iz(fi(mi));

if mi-fmin> lim & fmax-mi>lim
     res = mi;
   
end

if mi-fmin>lim
   res1 = rec_find_oct(Xfftz,iz,mi,fmin,lim); 
   res = [mi res1];
end

if fmax-mi>lim
    res2 = rec_find_oct(Xfftz,iz,fmax,mi,lim);
    res = [res res2];    
end


%--
function oct = determine_octave(x_data)
Xfft = abs(fft(x_data,2^15));
fftlen = length(Xfft)/2;
fs = 44100;
scale = 0.5*fs/fftlen;

Xfftz = 20*log10(Xfft(round(20/scale):round(5000/scale)));
m = max(Xfftz);
iz = find_all_peaks(Xfftz',1);

lim = 25/scale;

res = rec_find_oct(Xfftz,iz,5000/scale,20/scale,lim);

res = sort(res);

iz = res;
num_of_peaks = 30;

[pv,fi]=findpeaks(Xfftz(iz),num_of_peaks);
fi = sort(fi);

qw = diff(iz(fi));
qwi = find(qw<200/scale);
qw = qw(qwi);
[N,x] = hist(qw,20);
ni = find(x>20/scale);
[m,mi]=max(N(ni));
oct = x(ni(mi))*scale;
i=0;


%--
function y = smooth(x)

x = x';
[m,n] = size(x);

K = 10;
wq = sum(hanning(round(27.5/scale)));
for i=1:m
   x(i,:)=filter(hanning(5).*wq,1,x(i,:)); 
end

y = x';


%--
function oct = determine_octave2(x_data)
Xfft = abs(fft(x_data,2^15));
fftlen = length(Xfft)/2;
fs = 44100;
scale = 0.5*fs/fftlen;

Xfftz = 20*log10(Xfft(round(20/scale):round(1000/scale)));

m = max(Xfftz);
iz = find_all_peaks(Xfftz',1);

dbcutoff = m-5;
num_of_peaks = 20;

while length(find(Xfftz(iz)>dbcutoff))<num_of_peaks
   dbcutoff=dbcutoff-5; 
end

fi = find(Xfftz(iz)>dbcutoff);

qw = diff(iz(fi));
[N,x] = hist(qw,ceil(max(qw)/min(qw)));
ni = find(x>20);
[m,mi]=max(N(ni));
oct = x(ni(mi))*scale;
i=0;

% ---
function [peakv,peaki] = findpeaks(data,n)
ampq = data;
peakv = [];
peaki = [];
for i=1:n
    [m,mi] = max(ampq);
    peakv = [peakv m];
    peaki = [peaki mi];
    ampq(mi) =min(ampq);
end

