## Copyright (C) 2019 Jens
## 
## This program is free software: you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see
## <https://www.gnu.org/licenses/>.

## -*- texinfo -*- 
## @deftypefn {} {@var{retval} =} peak_detect (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens <jens@orion>
## Created: 2019-05-20

function retval = peak_detect ()

frames = load ('-ascii', './frames.dat');
[numFrames, N] = size(frames)

DO_SIM_PEAKS = 0;

Nh = N;
kn = 0.1
ks = 0.5;

pa = 0.3;
d_max = 30;

am = 0.04;
as = 0.2;
Ad = 20;
Cm = 0;

pb_min = 5
pb_max = 37

s = zeros(1, Nh);
a = zeros(1, Nh);
Xm = zeros(1, Nh);
Xs = zeros(1, Nh);
peak_count = zeros(1, Nh);

close all;
figure

for k=1:numFrames
	% Peak generation
	
	if DO_SIM_PEAKS
		z = fix(Nh*rand() + 1);
		
		if (rand() < pa)
			if (s(z) == 0)
				a(z) = ks;
				s(z) = fix(d_max*rand());
			end
		end
		x = kn*randn(1, N);
		for n = 1:Nh
			if s(n) > 0
				s(n) = s(n) - 1;
				f = 0.5-rand() + n;
				x = x + a(n)*cos(2*pi*f*(0:N-1)/N);
			end
		end
		X = abs(fft(x))/sqrt(N);
		X = X(1:Nh);
		X = 10*log10(X.*X);
	else
		X = frames(k,:);
		X = X(1:Nh);
	end
	
	% Init 
	if k == 1
		Xm = Xm + mean(X);
		Xs = Xs + mean(X);
	endif
	
	% Peak detection
	Xs = (1-as)*Xs + as*X;
	gate_peak = (X-Xm) >= Ad;
	peak_count = gate_peak.*peak_count + gate_peak;
	
	peaks = peak_count > 0;
	peak_pos = find(peaks);
	
	% Output
	numPeaksLast = 0;
	while(1)
		numPeaks = 0;
		for x = peak_pos,
			while(1)
				y0 = X(x);
				xp = min(Nh, x+1);
				xn = max(1, x-1);
				if X(xp) > y0
					peaks(x) = 0;
					peaks(xn) = 0;
					x = xp;
				elseif X(xn) > y0
					peaks(x) = 0;
					peaks(xp) = 0;
					x = xn;
				else
					peaks(xn) = 0;
					peaks(xp) = 0;
					numPeaks = numPeaks + 1;
					break;
				end	
			end
		endfor
		if numPeaks == numPeaksLast
			break;
		end
		numPeaksLast = numPeaks;
	end
	peak_pos = find(peaks);
	
	% Sort peaks
	[v, n] = sort((X-Xm)(peak_pos), 'descend');
	
	peak_cand_sorted = peak_pos(n);

	% Construct peak boxes
	peak_pos = [];
	peak_boxes_rel = zeros(1, Nh);
	peak_boxes_abs = zeros(1, Nh);
	for x = peak_cand_sorted,
		
		% Find peak widths
		left = x;
		right = x;
		nom = X(x) - 10;
		left_found = 0;
		right_found = 0;
		while(~(left_found & right_found))
			% Determine peak width left from center (x)
			if X(left) > nom
				if left > 1
					left = left - 1;
				else
					left_found = 1;
				end
			else
				left_found = 1;
			endif
			% Determine peak width right from center (x)
			if X(right) > nom
				if right < Nh
					right = right + 1;
				else
					right_found = 1;
				end
			else
				right_found = 1;
			endif
			pbh_left = x - left;
			pbh_right = right - x;
			
			% Take larger peak width
			pbh = max(pbh_left, pbh_right);
			
			% Ensure peak box is at least pb_min
			pbh = max(pbh, pb_min);
		end
		
		% Use peak width for peak box
		pbox_start = max(1, x-pbh);
		pbox_end = min(Nh, x+pbh);
		
		% Find intersection
		box_height_rel = X(x) - Xm(x);
		box_height_abs = X(x);
		does_intersect = 0;
		for n=pbox_start:pbox_end
			if peak_boxes_rel(n) > box_height_rel
				does_intersect = 1;
				break
			end
		endfor
		if does_intersect == 0
			peak_boxes_rel(pbox_start:pbox_end) = box_height_rel * ones(1, pbox_end - pbox_start + 1);
			peak_boxes_abs(pbox_start:pbox_end) = box_height_abs * ones(1, pbox_end - pbox_start + 1);
			peak_pos = [peak_pos x];
		end
	endfor

	% Update Xm at no-peak bins
	Xm = (1-am)*Xm + am*X;

	% Fill peak gaps with guessed values
	last = Xm(1);
	for n=2:Nh,
		if (Xm(n) < (last+2)) or (peak_boxes_rel(n) == 0);
			last = (1-am)*last + am*Xm(n);
		else
			Xm(n) = last;	
		end
	end
 
	% Create peak boxes with correct absolute height
	peak_box_values = Xm;
	peak_box_values((peak_boxes_rel > 0)) = peak_boxes_abs(peak_boxes_rel > 0);
	
	% Output
	subplot(2, 1, 1)
	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_box_values, 'm-'); grid; axis([0, Nh, -60, 60]); legend('X', 'Peaks', 'Peak Box')
	subplot(2, 1, 2)
	plot(1:Nh, Xs-Xm, 1:Nh, Xm); grid; axis([0, Nh, -60, 60]); legend('X_s - Xm', 'X_m')
	pause(1/30);
end

endfunction