% [freq,fase,amp] = freqmat2(allpeaks,allfase,allamps, delta)
% allpeaks = matrix of peaks returned from stft and normalized to freq 
% INPUT - allpeaks is the matrix of all the frequencies, with each column
% corresponding to a frame (or stft computation) allfase and allamps are
% corresponding matrics of all the phases and amplitudes 
% OUTPUT - freq, fase, and amp are matrices with each column holding tracks.
% A 0 in amps after data means a track going has died, and a track is born at
% the point data begins following a zero. Note: there is still a one-to-one
% correspondence between the frame number and the row index.

function [freq, fase, amp] = freqmat2(allpeaks, allfase, allamps, delta)
   
[numPeaks, numFrames] = size (allpeaks);
    
% pre-allocate space
freq = zeros(numFrames, numPeaks);
fase = zeros(numFrames, numPeaks);
amp = zeros(numFrames, numPeaks);

%set first row to first frame's peaks
freq(1,:) = allpeaks(:, 1)';
fase(1,:) = allpeaks(:, 1)';
amp(1,:) = allpeaks(:, 1)';
oldCrib = find(amp(1,:) == 0);
oldBirth = length(oldCrib);



extras = 0;  % keeps track of how many columns past numPeaks exist

for k = 1:(numFrames-1)

  %reset flag vector
  flag = zeros(numPeaks, 1);
  index = 0;  % in case nothing picked up

  % grab all the next frame's peak that fall in delta fan for the first peak
  if (freq(k,1) ~= 0)

    compare = (allpeaks(:, k+1) > (freq(k,1) - delta)) & (allpeaks(:, k+1) <= freq(k,1) + delta);
    possibles = find(compare);
    if possibles == []
      % nothing matches, this track DIES
      %freq(k+1, 1) = freq(k, 1); % for interpolating
      %fase(k+1, 1) = fase(k, 1); %for interpolating
    else  % find which is closest 
      [value, index] = min(abs(allpeaks(possibles, k+1) - freq(k, 1)));
      index = possibles(index);
    end % end if

  end % end if


  for i = 1:(numPeaks - 1 + extras)
    oldCompare = compare;
    oldIndex = index;  index = 0;
    oldValue = value;
    if (amp(k,i+1) ~= 0)  % changed from freq
    
      % check if next guy doesn't want that point

% Side note to users- we are only checking if the next track does not want
% that point, this does not provide total coverage/correctness, but
% there is no way to produce exactitude in this, and the marginal value 
% of checking other guys goes down quickly, so this should prove good
% To improve:  try using a small delta, and then a bigger delta.  For our tests
% a delta from .03 to .05 seemed to provide the best results.

      %calculate for next guy
     
      compare = ((allpeaks(:, k+1) > (freq(k,i+1) - delta)) & (allpeaks(:, k+1) <= freq(k,i+1) + delta));
     
        % check if someone else has grabbed those points
  
      possibles = find(compare);
      for q = 1:length(possibles)
        if (flag(possibles(q))==1) % flag is set, that peak is taken
          compare(possibles(q)) = 0;  % erase from compare
        end % end if
      end % end for
	     
      possibles = find(compare); %may have changed
      if possibles == []
        % nothing matches, this track DIES
        %freq(k+1, i) = freq(k, i); % for interpolating
        %fase(k+1, i) = fase(k, i); %for interpolating
      else  % find which is closest 
        [value, index] = min(abs(allpeaks(possibles, k+1) - freq(k, i+1)));
        index = possibles(index);
      end % end  
    end % end if next freq != 0   
      
    if(amp(k, i) ~=0)% used to be freq 
      if (index==oldIndex & oldIndex~=0  & oldValue>value) % oldguy takes second choice
                                                           % only if new guy is closer.
        oldCompare(oldIndex) = 0; 
       
        % check flag again

        oldPos = find (oldCompare);
        for q = 1:length(oldPos)
          if (flag(oldPos(q))==1) % flag is set, that peak is taken
            oldCompare(oldPos(q)) = 0;  % erase from compare
          end
        end

        oldPos  = find(oldCompare);

        if (oldPos == []) % nothing matches, this track DIES
          %freq(k+1, i) = freq(k, i); % for interpolating
          %fase(k+1, i) = fase(k, i); %for interpolating
	  oldIndex = 0;

        else  % find which is closest 
          [oldValue, oldIndex] = min(abs(allpeaks(oldPos, k+1) - freq(k, i)));
          oldIndex = oldPos(oldIndex);
        end % end if
      end % end if 
      
      if (oldIndex ~= 0) 

        freq(k+1, i) = allpeaks(oldIndex, k+1);
        fase(k+1, i) = allfase(oldIndex, k+1);
        amp(k+1, i) = allamps(oldIndex, k+1);
        flag(oldIndex) = 1;
        
      end
    end %end if
  end % end for(i) loop


% last guy auto gets whatever one he wants: 
  if(index ~= 0)
    freq(k+1, numPeaks + extras) = allpeaks(index, k+1);
    fase(k+1, numPeaks + extras) = allfase(index, k+1);
    amp(k+1,  numPeaks + extras) = allamps(index, k+1);
    flag(index) = 1; 
  end 


% Now, all the existing tracks have gotten to pick successors from the frame
% but there may be values in frame k+1 not matched into k, they need to be born
% and added to open space somewhere at the correct row index to start a track.
% Check the flag vector for values not matched, and birth them.

    for y=1:numPeaks
      if (flag(y) == 0 & oldBirth ~= 0) % 
        newBorn = oldCrib(oldBirth);
        if(allpeaks(y, k+1) ~= 0)  % don't birth zeros
          freq(k+1, newBorn) = allpeaks(y, k+1); % A track is born!
          %freq(k, newBorn) = allpeaks(y, k+1);  % for interpolating
          amp(k+1, newBorn) = allamps(y, k+1); 
          fase(k+1, newBorn) = allfase(y, k+1);
          %fase(k, newBorn) = allfase(y, k+1);   % for interpolating
          oldBirth = oldBirth - 1;
        end
      
      elseif (flag(y) == 0) % need to add new track column
	
        extras = extras + 1; newC = numPeaks + extras;
        freq(k+1, newC) = allpeaks(y, k+1);
        %freq(k, newC) = allpeaks(y, k+1); % for interpolating
        amp(k+1, newC) = allamps(y, k+1); 
        fase(k+1, newC) = allfase(y, k+1);
        %fase(k, newC) = allfase(y, k+1);  % for interpolating
      end % end if
    end % end local for

    oldCrib = find (amp(k+1,:) == 0); % prepare for next cycle  
    oldBirth = length(oldCrib);       % OldCrib holds indices of zeros
     %                                % from k+1 track and oldBirth is how many
  end % end frame (k) loop for