%FUNCTION: [FF,FB]=smfmtk(fm,bw) smoothes the formant track.
%
% Input : fm == the formant frequency track
%         bw == the formant bandwidth track
% Output: FF == the smoothed formant frequency track    
%         FB == the smoothed formant bandwidth track
%         Fmrt == the smoothed formant roots

function [FF,FB,Fmrt]=smfmtk(fm,bw);

[nr,nc]=size(fm);

nfm=zeros(1,nr); % vector of number of formants
Fmrt=zeros(nr,10);

if nc<5
   disp('The number of formants should be 5.');
   error('The size of input matrix is Wrong in using "smfmtk"! ');
end

  %-----------------------%
  % 1. check the formants %
  %-----------------------%

   for kf=1:nr
 
          % throw away those fromants whose
          %   a. bandwidth exceed 700 Hz
          %   b. bandwidth to formant ratio is bigger than .8
          %   c. formant exceeds 4700 Hz
          %   d. formant below 200 Hz
          %   e. bandwidth exceeds 1000 Hz and has a 500Hz neighbor

          ff=fm(kf,:);
          bb=bw(kf,:);
          fm(kf,:)=zeros(1,5);
          bw(kf,:)=zeros(1,5);
          nrr=length(ff);

          for ii=1:nrr
             if ff(ii)<2000 & bb(ii)>700
                 ff(ii)=0;
                 bb(ii)=0;
             elseif bb(ii)/(ff(ii)+1)>0.8
                 ff(ii)=0;
                 bb(ii)=0;
             elseif ff(ii)>4700
                 ff(ii)=0;
                 bb(ii)=0;
             elseif ff(ii)<200
                 ff(ii)=0;
                 bb(ii)=0;
             elseif bb(ii)>1000
                 if bb(ii)>1570
                    ff(ii)=0;
                    bb(ii)=0;
                 elseif ( ff(ii)-ff(ii-1) )<500
                    ff(ii)=0;
                    bb(ii)=0;
                 elseif ii<nrr 
                    if( ff(ii+1)-ff(ii) )<500
                      ff(ii)=0;
                      bb(ii)=0;
                    end
                 end
             end
          end
          idx=find(ff>0);
          nff=length(idx);
          ff=ff(idx);
          bb=bb(idx);
          [ff,idx]=sort(ff);
          bb=bb(idx);

         % get rid of spurious root
         %   a. more than 4 roots on the left-hand-side ( >2500 Hz )
         %   b. more than 4 roots on the right-hand-sidr (<2500 Hz )
         if nff>=4
            chka=find(ff>2500);
            if length(chka)>3
              [dum,ii]=max(bb(chka));
              ii=ii+( nff-length(chka) );
              bb(ii)=[];
              ff(ii)=[];
              nff=nff-1;
            elseif (nff-length(chka)) >3
              chkb=find(ff<2500);
              [dum,ii]=max(bb(chkb));
              bb(ii)=[];
              ff(ii)=[];
              nff=nff-1;
            end
         end

         % delete the largest distance if more than 5 frmts are selected
         if nff>5
             off=fm(kf-1,:);
             for ii=1:nff
                 dis(ii)=min( abs( ff(ii)-off ) );
             end
             [dum,ii]=max(dis);
             ff(ii)=[];
             bb(ii)=[];
             nff=5;
         end
         fm(kf,1:nff)=ff;
         bw(kf,1:nff)=bb;
         nfm(kf)=nff;
          
  end

  %--------------------------------------------------%
  % 2. allocate the formants in an appropriate order %
  % find full frame ( with five formants)            %
  %--------------------------------------------------%
  first=1;
  while nfm(first)~=5
      first=first+1;
  end

  % from 1 to first full frame
  off=fm(first,:); % standard formant distribution

  for kf=1:first-1
      tff=fm(kf,1:nfm(kf)); % formant of current frame
      tbb=bw(kf,1:nfm(kf));

      for ii=1:nfm(kf)
          dis=abs(tff(ii)-off);
          [dum,jj]=min(dis);
          off(jj)=10000;
      end
      zdx=find(off~=10000);
      fdx=find(off==10000);
      if length(zdx)~=( 5-nfm(kf) )
         disp('Error of allocate the formants!')
         tff
         zdx
      else
         fm(kf,zdx)=zeros(1,length(zdx));
         bw(kf,zdx)=zeros(1,length(zdx));
         fm(kf,fdx)=tff;
         bw(kf,fdx)=tbb;
      end
      off(fdx)=tff;
  end

  % find the last full frame
  last=nr;
  while nfm(last)~=5
       last=last-1;
  end

  % from last full frame to nr
  off=fm(last,:); % standard formant distribution
  for kf=last+1:nr
      tff=fm(kf,1:nfm(kf)); % formant of current frame
      tbb=bw(kf,1:nfm(kf));

      for ii=1:nfm(kf)
          dis=abs(tff(ii)-off);
          [dum,jj]=min(dis);
          off(jj)=10000;
      end
      zdx=find(off~=10000);
      fdx=find(off==10000);
      if length(zdx)~=( 5-nfm(kf) )
         disp('Error!')
         tff
         zdx
      else
         fm(kf,zdx)=zeros(1,length(zdx));
         bw(kf,zdx)=zeros(1,length(zdx));
         fm(kf,fdx)=tff;
         bw(kf,fdx)=tbb;
      end
      off(fdx)=tff;
  end

  %from first full frame to last full frame
  xx1=first;

  off=fm(xx1,:); % standard formant distribution
  for kf=first+1:last-1
      if nfm(kf)==5
         off=fm(kf,:);
      else
         tff=fm(kf,1:nfm(kf)); % formant of current frame
         tbb=bw(kf,1:nfm(kf));

         for ii=1:nfm(kf)
            dis=abs(tff(ii)-off);
            [dum,jj]=min(dis);
            off(jj)=10000;
         end
         zdx=find(off~=10000);
         fdx=find(off==10000);
         if length(zdx)~=( 5-nfm(kf) )
           disp('Error!')
           tff
           zdx
         else
           fm(kf,zdx)=zeros(1,length(zdx));
           bw(kf,zdx)=zeros(1,length(zdx));
           fm(kf,fdx)=tff;
           bw(kf,fdx)=tbb;
         end
         off(fdx)=tff; 
      end
  end



for loop=1:2
  %----------------------------%
  % 3. fill out the empty slot %
  %----------------------------%
   
  % from frame first to last
for jj=1:5
    for kf=first+1:last-1
        if fm(kf,jj)~=0
           xx1=kf;
        else
           xx2=kf+1;
           while fm(xx2,jj)==0
               xx2=xx2+1;
           end
           w2=1/(xx2-xx1)/(xx2-xx1);
           w1=1-w2;
           fm(kf,jj)=w1*fm(xx1,jj)+w2*fm(xx2,jj);
           bw(kf,jj)=w1*bw(xx1,jj)+w2*bw(xx2,jj);
           xx1=kf;
        end
    end
end

  % from frame 1 to first
 for k=1:5
    xx2=first;
    if fm(1,k)==0
       fm(1,k)=0.5*fm(xx2,k)+0.5*fm(xx2+1,k);
       bw(1,k)=0.5*bw(xx2,k)+0.5*bw(xx2+1,k);
    end
    xx1=1;
    for kf=2:first-1
       if fm(kf,k)~=0
          xx1=kf;
       else
          w2=1/(xx2-xx1)/(xx2-xx1);
          w1=1-w2;
          fm(kf,k)=w1*fm(xx1,k)+w2*fm(xx2,k);
          bw(kf,k)=w1*bw(xx1,k)+w2*bw(xx2,k);
          xx1=kf;
       end
    end
 end

 % from frame last to nr
 for k=1:5
    xx1=last;
    if fm(nr,k)==0
       fm(nr,k)=0.5*fm(last,k)+0.5*fm(last-1,k);
       bw(nr,k)=0.5*bw(last,k)+0.5*bw(last-1,k);
    end
    xx1=1;
    for kf=nr-1:-1:last
       if fm(kf,k)~=0
          xx2=kf;
       else
          w2=1/(xx2-xx1)/(xx2-xx1);
          w1=1-w2;
          fm(kf,k)=w1*fm(xx1,k)+w2*fm(xx2,k);
          bw(kf,k)=w1*bw(xx1,k)+w2*bw(xx2,k);
          xx2=kf;
       end
    end
 end

  %------------------------------%
  % 4. delete the  spurious root %
  %------------------------------%
for kf=first:last
    dff=diff(fm(kf,:));
    chk=find(dff<220);
    if chk~=[]
       for ii=1:length(chk)
          xx1=chk(ii);
          d1=abs( fm(kf-1,xx1)-fm(kf,xx1) );
          d2=abs( fm(kf-1,xx1+1)-fm(kf,xx1+1) );
            if d1>d2
              fm(kf,xx1)=0;
              bw(kf,xx1)=0;
            else
              fm(kf,xx1+1)=0;
              bw(kf,xx1+1)=0;
            end
       end
       nfm(kf)=5-length(chk);
    else
       nfm(kf)=5;
    end
end

  % find full frame ( with five formants)
  first=1;
  while nfm(first)~=5
      first=first+1;
  end

  % find the last full frame
  last=nr;
  while nfm(last)~=5
       last=last-1;
  end

end %% for loop=1:2

  %----------------------------%
  % 5. fill out the empty slot %
  %----------------------------%
   
  % from frame first to last
for k=1:5
    xx1=first;
    for kf=first+1:last-1
        if fm(kf,k)~=0
           xx1=kf;
        else
           xx2=kf+1;
           while fm(xx2,k)==0
              xx2=xx2+1;
           end
           w2=1/(xx2-xx1)/(xx2-xx1);
           w1=1-w2;
           fm(kf,k)=w1*fm(xx1,k)+w2*fm(xx2,k);
           bw(kf,k)=w1*bw(xx1,k)+w2*bw(xx2,k);
           xx1=kf;
        end
    end
end

 %---------------------------------------%
 % 6. convert fm & bw into formant roots %
 %---------------------------------------%

FF=fm;
FB=bw;

for kf=1:nr
    the=FF(kf,:)*pi/5000;
    %rdis=exp(-1.0*FB(kf,:)*pi/10000);
    tmp=cos( bw(kf,:)*pi/10000 );
    for k=1:length(tmp)
        tr=roots([1 2*tmp(k)-4 1]);
        tr=tr( tr<1 & tr>-1 );
        if length(tr)~=1
           tr=exp(-1.0*FB(kf,k)*pi/10000);
        end
        rdis(k)=tr;   
    end
    rr=rdis.*exp(j*the);
    Fmrt(kf,:)=[rr conj(rr)]; 
end