## 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', '/home/jens/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.9;
Ad = 20;
Cm = 0;
pbh = 10;

s = zeros(1, Nh);
a = zeros(1, Nh);
Xm = -120*ones(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
	
	% Peak detection
	Xs = (1-as)*Xs + as*X;
	gate_peak = Xs > (Ad+Xm);
	gate_upd = Xs > (10+Xm);
	peak_count = gate_peak.*peak_count + gate_peak;
	peak_cand = peak_count > 0;
	% Update Xm at no-peak bins
	Xm = (1-gate_upd) .* ((1-am)*Xm + am*X) + gate_upd.*Xm*0.7;
	
	% Fill peak gaps with guessed values
	last = Xm(1);
	for n=2:Nh,
		if gate_upd(n) == 0
			last = Xm(n);
		else
			Xm(n) = last;	
		end
	end
 
	% Output
	numPeaksLast = 0;
	while(1)
		numPeaks = 0;
		for x = find(peak_cand > 0),
			_x = x;
			while(1)
				y0 = X(_x);
				xp = min(Nh, _x+1);
				xn = max(1, _x-1);
				if X(xp) > y0
					peak_count(_x) = 0;
					peak_count(xn) = 0;
					_x = xp;
				elseif X(xn) > y0
					peak_count(_x) = 0;
					peak_count(xp) = 0;
					_x = xn;
				else
					peak_count(xn) = 0;
					peak_count(xp) = 0;
					numPeaks = numPeaks + 1;
					break;
				end	
			end
		endfor
		if numPeaks == numPeaksLast
			break;
		end
		numPeaksLast = numPeaks;
	end
	peak = peak_count > Cm;
	marker_Y = X.*peak;
	marker_X = find(peak > 0);
	
	% Construct peak boxes
	peak_boxes = Xm;
	for x = find(peak > 0),
			pbox_start = max(1, x-pbh);
			pbox_end = min(Nh, x+pbh);
			peak_boxes(pbox_start:pbox_end) = X(x) * ones(1, pbox_end - pbox_start + 1);
	endfor
	
	subplot(2, 1, 1)
	plot(1:Nh, X, marker_X, marker_Y(marker_X), 'ro', 1:Nh, peak_boxes, 'm-'); grid; axis([0, Nh, -60, 40]);
	subplot(2, 1, 2)
	plot(1:Nh, Xs, 1:Nh, Xm); grid; axis([0, Nh, -60, 40]);
	pause(1/30);
end

function s = matchedFilter(N, B)
n2 = (N-1)/2
n = (0:N-1)-n2
s = sinc(n/B)

endfunction
	
endfunction