## 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 ()

N = 1024
Nh = N/2;
kn = 0.1

close all;
figure

pa = 0.3;
d_max = 30;

s = zeros(1, Nh);
a = zeros(1, Nh);
Xm = zeros(1, Nh);
Xs = zeros(1, Nh);
peak_count = zeros(1, Nh);
am = 0.1;
as = 0.9;

for k=1:64
	% Peak generation
	f = fix(N/2*rand());
	k = f + 1;
	
	if (rand() < pa)
		if (s(k) == 0)
			a(k) = 1.0;
			s(k) = fix(d_max*rand());
		end
	end
	x = kn*randn(1, N);
	for n = 1:Nh
		if s(n) > 0
			s(n) = s(n) - 1;
			x = x + a(n)*cos(2*pi*n*(0:N-1)/N);
		end
	end
	X = abs(fft(x))/sqrt(N);
	X = X(1:Nh);
	X = 10*log10(X.*X);

	% Peak detection
	Xs = (1-as)*Xs + as*X;
	Xm_gate = Xs > (10+Xm);
	peak_count = Xm_gate.*peak_count + Xm_gate;
	peak = peak_count > 1;
	% Update Xm at no-peak bins
	Xm = (1-peak) .* ((1-am)*Xm + am*X) + peak.*Xm;
	
	% Output
	marker_X = find(peak ~= 0);
	marker_Y = X.*peak;
	subplot(2, 1, 1)
	plot(1:Nh, X, marker_X, marker_Y(marker_X), '+'); 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