function [ rr_list ] = find_r_peaks( ecg_vector, samplerate )
%FIND_R_PEAKS Detect RR peaks according to method by Benitez et al.
%   
% Author Jan Kraemer <camel@gmx.de>
%

ecg_vector = ecg_vector(:);
nan_vector = isnan(ecg_vector);
ecg_vector(nan_vector) = 0;

nyquistrate = samplerate / 2;
if samplerate < 500
    window_length = 1024;
elseif samplerate < 1000
    window_length = 2048;
elseif samplerate < 2000
    window_length = 4096;
else
    window_length = 8192;
end


filter_coeff = fir1(20,[8 20] ./ nyquistrate,'DC-0',blackman(21));
filtered_ecg_gradient = gradient(filtfilt(filter_coeff, 1, ecg_vector));

rr_list = zeros(0,1);

ecg_length = length(ecg_vector);

window_start = 1;
window_end = min(ecg_length, window_start + window_length);

last_start = 0;

while window_start < ecg_length && window_start > last_start && window_end > window_start
%    fprintf('(%08i->%08i) of %08i\n', window_start, window_end, ecg_length);
%    fprintf('%08i found R-Peaks\n', length(rr_list));
%    fprintf('window_start < ecg_length := %i && window_start > last_start := %i && window_end > window_start =: %i\n', window_start < ecg_length, window_start > last_start, window_end > window_start);
    last_start = window_start;
    
    ecg_segment = ecg_vector(window_start:window_end);
    feg_segment = filtered_ecg_gradient(window_start:window_end);
    
    transformed_segment = imag(hilbert(feg_segment));
    max_transformed = max(transformed_segment);
    
    rms_transformed=rootmeansquare(transformed_segment);

    
    if (rms_transformed == 0) || isnan(rms_transformed) || isnan(max_transformed)
        % WTF???
%        disp('WTF?');
        window_start = window_start + window_length;
        window_end = min(ecg_length, window_start + window_length);
        continue;
    end
    
    thresholds = zeros(0,1);
    if rms_transformed >= (0.18 * max_transformed)
        thresholds(end+1) = 0.39*max_transformed;
    end
    if window_start > 1 && max_transformed > (2 * last_max)
        thresholds(end+1) = 0.39 * last_max;
    end
    thresholds(end+1) = 1.6*rms_transformed;
    

    segment_threshold = max(thresholds);
    
    last_max = max_transformed;
    
    
    peak_selection = transformed_segment >= segment_threshold;
    % it is important to have a 0 at first and last spot to have matching
    % start/end positions in this method of finding them
    peak_selection(1) = false;
    peak_selection(end) = false;
    
    peak_intervals = diff(0+peak_selection);

    start_interval = find(peak_intervals == 1) + 1;
    end_interval = find(peak_intervals == -1);

    peak_list = zeros(0,1);
    

    % The last found peak will be handeled in the next segment    
    n_last_interval = min([length(start_interval) length(end_interval)]);
    for i = 1:n_last_interval
        if (end_interval(i) - start_interval(i) < 3)
            start_interval(i) = start_interval(i) - 1;
            end_interval(i) = end_interval(i) + 1;
        end
        [pks, locs] = findpeaks(transformed_segment(start_interval(i):end_interval(i)));
        peak_list(end+1) = locs(1) + start_interval(i) - 1;
        
    end
    
    peak_times = peak_list / samplerate;
    peak_diff = diff(peak_times);
    while any(peak_diff < 0.200)
        temp = find(peak_diff < 0.200);
        removed_peaks = zeros(0,1);
        for i = 1:length(temp)
            peak1 = peak_list(temp(i));
            peak2 = peak_list(temp(i) + 1);
            
            if transformed_segment(peak1) <= transformed_segment(peak2)
                removed_peaks(end+1) = temp(i);
            elseif transformed_segment(peak2) <= transformed_segment(peak1)
                removed_peaks(end+1) = temp(i)+1;
            end
            
            
        end
        %disp(['TOO CLOSE DIFFS!!!!!\n']);
        
        if isempty(removed_peaks)
            disp('WTF? No Peaks removed');
            break;
        end
        
        peak_list(removed_peaks) = [];
        
        peak_times = peak_list / samplerate;
        peak_diff = diff(peak_times);
    end

    
    if (length(peak_list) < 2) 
        % If there is only one peak found - ignore this segment
        % there must be too much noise somewhere...
        window_start = window_start + round(0.1 * samplerate);
        window_end = min(ecg_length, window_start + window_length);
        continue;
    end

    
    for i = 1:(length(peak_list) - 1)
        current_peak_start = max([1 (peak_list(i) - 10)]);
        current_peak_end = min([length(ecg_segment) (peak_list(i) + 10)]);

%           fprintf(1, '%i:%i\n', current_peak_start, current_peak_end);
        [who_cares, locs] = findpeaks(ecg_segment(current_peak_start:current_peak_end));

        if (current_peak_start + locs(1)) < round(0.1 * samplerate)
            continue;
        end
        rr_list(end+1) = locs(1) + (current_peak_start - 1) + (window_start - 1);
        if (rr_list(end) ~= round(rr_list(end)))
            error('rr_list contains non integer');
        end
    end
    
    
    window_start = window_start + peak_list(end) - round(0.2 * samplerate);
    window_end = min(ecg_length, window_start + window_length);
end
% fprintf('(%08i->%08i) of %08i\n', window_start, window_end, ecg_length);
% fprintf('window_start < ecg_length := %i && window_start > last_start := %i && window_end > window_start =: %i\n', window_start < ecg_length, window_start > last_start, window_end > window_start);
   
ecg_vector(nan_vector) = nan;




function [ pks, locs ] = findpeaks( vector )
%FINDPEAKS find maxima

pks = max(vector);
locs = find(vector >= pks, 1);


