%  Function to extract the beats per minute from a file of raw samples
%  It does signal filtering and peaks extraction.

 
function extract_bpm()


  NB_LPF_TAP = 16; % Number of coefficients of the low-pass FIR
  FS         = 64; % Sampling frequency, should be > 50 Hz (BPM unit is roughly 1/60 second)


  % Detect if using Octave and load additional package
  isOctave = exist('OCTAVE_VERSION', 'builtin') ~= 0;
  if (isOctave)
    pkg load signal
  end

  samples = load('samples.txt');
  
  
  % IIR LP filter: y[i] = alpha*x[i] + (1-alpha)*y[i-1];
  % alpha = DT/(RC + DT) or RC = DT(1-alpha)/alpha, DT = sampling period
  % fc = alpha/((1-alpha)*2*pi*DT);
  alpha = 0.1;

  iir_lpf_out(1) = samples(1);
  for k=1:length(samples)-1
    iir_lpf_out(k+1) = alpha*samples(k+1) + (1-alpha)*iir_lpf_out(k);
  end


  % FIR LP filter
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  fir_lpf_coeff = fir1(NB_LPF_TAP-1, 3.7/FS, window(@hamming,NB_LPF_TAP)); % cut at 220/60=3.7 Hz
  fir_lpf_out = filter(fir_lpf_coeff, 1, iir_lpf_out);
  fir_lpf_out = fir_lpf_out(NB_LPF_TAP+1:end); % remove initial samples


  % IIR HP filter
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % IIR HPF: y[i] = alpha*y[i-1] + alpha(x[i]-x[i-1])
  % alpha = RC/(RC + DT)
  alpha = 0.9;

  hpf_out(1) = fir_lpf_out(1);
  for k=1:length(fir_lpf_out)-1
    hpf_out(k+1) = alpha*hpf_out(k) + alpha*(fir_lpf_out(k+1)-fir_lpf_out(k));
  end
  hpf_out = hpf_out(70:end); % remove initial samples

  peaks_list = extract_peaks(hpf_out);

  mean_dist = mean(peaks_list(2:end,1)-peaks_list(1:end-1,1));
  median_dist = median(peaks_list(2:end,1)-peaks_list(1:end-1,1));

  disp(sprintf('mean: %g, median: %g', mean_dist, median_dist));
  disp(sprintf('BPM: %g\n', 60*FS/median_dist));


  % PLOTS
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  plot(samples);
  hold on
  plot(fir_lpf_out, 'r');
  hold off
  %print -dsvg fig1.svg
  %print -dpng fig1.png

  figure
  plot(hpf_out, 'b');
  hold on
  plot(peaks_list(:,1), peaks_list(:,2), 'r*');
  hold off
  %print -dsvg fig2.svg
  %print -dpng fig2.png

end


% Function extract_peaks: extracts the most positive peaks from a signal.
% A peak is the most positive value found between 2 crossings of the signal with the zero line.
% A filtering is applied to remove peaks that are too weak compared to the strongest peak.
% Output col1: list of peak indexes, col2: list of peak values
function peaks = extract_peaks(samples)

 % Accepted peaks must be stronger than PEAK_ACCEPT_RATIO*strongest_peak
  PEAK_ACCEPT_RATIO = 0.2;
  PEAK_MIN_DISTANCE = 15;

  peaks = [];
  peak_search_on = 0;
  local_peak = 0;
  local_peak_idx = 0;

  for k=2:length(samples)
    if peak_search_on==0 && samples(k)>0
      peak_search_on = 1;
    elseif peak_search_on==1 && (samples(k)<=0 || k==length(samples))
      peak_search_on = 0;
      peaks = [peaks ; [local_peak_idx local_peak]];
      local_peak     = 0;
    end
    
    if peak_search_on==1 &&  samples(k)>local_peak
      local_peak     = samples(k);
      local_peak_idx = k;
    end
  end

  % Peaks filter 1: remove lowest peaks
  highest_peak = max(peaks(:,2));
  removed_peaks_idx = find(peaks(:,2)<(highest_peak*PEAK_ACCEPT_RATIO));
  peaks(removed_peaks_idx,:) = [];

  % Peaks filter 2: remove peaks too close to bigger ones
  removed_peaks_idx = [];
  for k=1:size(peaks,1)-1
    if peaks(k+1,1)-peaks(k,1)<PEAK_MIN_DISTANCE
      if peaks(k+1,2)<peaks(k,2)  % keep the most negative value
        removed_peaks_idx = [removed_peaks_idx k];
      else
        removed_peaks_idx = [removed_peaks_idx k+1];
      end
    end
  end
  peaks(removed_peaks_idx,:) = [];

end