function [pitch, pitcherror] = TWM (iloc, ival, N, SR)
%function [pitch, pitcherror] = TWM (iloc, ival, N, SR)
%
% => Two-way mismatch error pitch detection
% using Bauchamp & Maher algorithm
%
% data:
% iloc: location (bin) of the peaks
% ival: magnitudes of the peaks
% N: number of peaks
% SR: sampling rate
ifreq = (iloc-1)/N*SR; % frequency in Hertz
%--- avoid zero frequency peak
[zvalue,zindex] = min(ifreq);
if (zvalue==0)
ifreq(zindex) = 1;
ival(zindex) = -100;
end
ival2 = ival;
[MaxMag,MaxLoc1] = max(ival2);
ival2(MaxLoc1) = -100;
[MaxMag2,MaxLoc2]= max(ival2);
ival2(MaxLoc2) = -100;
[MaxMag3,MaxLoc3]= max(ival2);
%--- pitch candidates
nCand = 10; % number of candidates
pitchc = zeros(1,3*nCand);
pitchc(1:nCand)=(ifreq(MaxLoc1)*ones(1,nCand))./((nCand ...
+1-[1:nCand]));
pitchc(nCand+1:nCand*2)=(ifreq(MaxLoc2)*ones(1,nCand))./ ((nCand ...
+1-[1:nCand]));
pitchc(nCand*2+1:nCand*3)=(ifreq(MaxLoc3)*ones(1,nCand))./((nCand ...
+1-[1:nCand]));
%pitchc=100:300;
harmonic = pitchc;
%--- predicted to measured mismatch error
ErrorPM = zeros(fliplr(size(harmonic)));
MaxNPM = min(10,length(iloc));
for i=1:MaxNPM
difmatrixPM = harmonic' * ones(size(ifreq))';
difmatrixPM = abs(difmatrixPM ...
-ones(fliplr(size(harmonic)))*ifreq');
%[FreqDistance,peakloc] = min(difmatrixPM,[],2);
difSize=size(difmatrixPM); % octave adapt.
clear FreqDistance % octave adapt.
for i=1:difSize(1), % octave adapt.
   [FreqDistance(i),peakloc(i)] = min(difmatrixPM(i,:)); % octave adapt.
end % octave adapt.
Ponddif = FreqDistance' .* (harmonic'.^(-0.5));
PeakMag = ival(peakloc);
MagFactor = max(0, MaxMag - PeakMag + 20);
MagFactor = max(0, 1.0 - MagFactor/75.0);
ErrorPM = ErrorPM ...
+(Ponddif+MagFactor.*(1.4*Ponddif-0.5));
harmonic = harmonic+pitchc;
end
%--- measured to predicted mismatch error
ErrorMP = zeros (fliplr(size(harmonic)));
MaxNMP = min(10,length(ifreq));
for i=1:length(pitchc)
nharm = round(ifreq(1:MaxNMP)/pitchc(i));
nharm = (nharm>=1).*nharm + (nharm<1);
clear FreqDistance % octave adapt.
FreqDistance = abs(ifreq(1:MaxNMP) - nharm*pitchc(i));
Ponddif = FreqDistance.* (ifreq(1:MaxNMP).^(-0.5));
PeakMag = ival(1:MaxNMP);
MagFactor = max(0,MaxMag - PeakMag + 20);
MagFactor = max(0,1.0 - MagFactor/75.0);
ErrorMP(i) = sum(MagFactor.*(Ponddif ...
+MagFactor.*(1.4*Ponddif-0.5)));
end
%--- total error
Error = (ErrorPM/MaxNPM) + (0.3*ErrorMP/MaxNMP);
[pitcherror, pitchindex] = min(Error);
pitch = pitchc(pitchindex);