## 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
	% ----------------------
	% Update of Xs
	Xs = (1-as)*Xs + as*X;
	
	% Update Xm at no-peak bins
	Xm = (1-am)*Xm + am*X;

	% Conditional update of Xm
	last = Xm(1);
	for n=2:Nh,
		if Xm(n) < (last+2)
			last = (1-am)*last + am*Xm(n);
		else
			Xm(n) = last;	
		end
	end
	
	% Create peak distances by subtracting Xm
	Xd = (X-Xm);

	% Find peak candidates by thresholding
	peaks = Xd >= Ad;
	peak_pos = find(peaks);
	
	% Search true maximum peak using hill climbing
	numPeaksLast = 0;
	while(1)
		numPeaks = 0;
		for pos = peak_pos,
			while(1)
				y0 = X(pos);
				pos_p = min(Nh, pos+1);
				pos_n = max(1, pos-1);
				if X(pos_p) > y0
					peaks(pos) = 0;
					peaks(pos_n) = 0;
					pos = pos_p;
				elseif X(pos_n) > y0
					peaks(pos) = 0;
					peaks(pos_p) = 0;
					pos = pos_n;
				else
					peaks(pos_n) = 0;
					peaks(pos_p) = 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((Xd)(peak_pos), 'descend');
	
	peak_pos_sorted = peak_pos(n);

	% Construct peak boxes
	peak_pos = [];
	peak_boxes_abs = Xm;
	peak_boxes_rel = zeros(1, Nh);
	for pos = peak_pos_sorted,
		
		% Find peak widths
		left = pos;
		right = pos;
		nom = 10;
		left_found = 0;
		right_found = 0;
		pbh = 0;
		while(~(left_found & right_found))
			% Determine peak width left from center (pos)
			if Xd(left) > nom
				if left > 1
					left = left - 1;
				else
					break;
				end
			else
				left_found = 1;
			endif
			% Determine peak width right from center (pos)
			if Xd(right) > nom
				if right < Nh
					right = right + 1;
				else
					break;
				end
			else
				right_found = 1;
			endif
			pbh_left = pos - left;
			pbh_right = right - pos;
			
			% Take larger peak width
			pbh = max(pbh_left, pbh_right);
			
			% Ensure peak box is at least pb_min
			pbh = max(pbh, pb_min);
			
			if pbh > pb_max
				pbh = 0;
				break;
			end	
		end
		
		if pbh == 0
			continue;
		endif

		% Use peak width for peak box
		pbox_start = max(1, pos-pbh);
		pbox_end = min(Nh, pos+pbh);
		
		% Find intersection
		box_height_abs = X(pos);
		box_height_rel = Xd(pos);
		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_abs(pbox_start:pbox_end) = box_height_abs * ones(1, pbox_end - pbox_start + 1);
			peak_boxes_rel(pbox_start:pbox_end) = box_height_rel * ones(1, pbox_end - pbox_start + 1);
			peak_pos = [peak_pos pos];
		end
	endfor

	% Output
	subplot(2, 1, 1)
	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_boxes_abs, '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