function [ freqRespWaymarkA, waymarkDelay ] = waymarkDelayCalc(c0, rxtxSpeedMax, waymarkInit, freqRespWaymarkA, field, fieldPropDelayFirst, T_DECI_SAMPLE, T_WAYMARK, freqSearch)
% WAYMARKCALC Calculate Waymark response and calculates the delays
% This function takes the setup as arguments and generates the pressure field and the delays through the waymark

% The input arguments are:
% previously calculated frequency responses for the delay search,
% num. of samples in the Waymark interval,
% sample rate,
% freqency sweep parameters

% The outputs are:
% frequency response for the delay search,
% delays between Waymarks, at the decimated sample rate,
% Waymark delay

	% Delay Adjustment parameters
	NUM_ITER			= 10;		% Number of iterations for dichotomous search

	% Search in the frequency domain for the approprate delay to apply.
	D_INIT_DIV	= (T_DECI_SAMPLE/16);
	deltaSteps	= max(2*ceil(T_WAYMARK*rxtxSpeedMax*2/c0/D_INIT_DIV), 8);	% If T_DECI_SAMPLE is reletivly large compared to T_WAYMARK then make sure there are some steps in the search
	delta = linspace(-(T_WAYMARK*rxtxSpeedMax*2/c0), (T_WAYMARK*rxtxSpeedMax*2/c0), deltaSteps);	% Possible interval for the change of propagation delays
	% Flag for the plot debug
	plotSearch	= false;
																																			
																																			
	% Delay search
	% ============
	% Perform a search through the delays checking the correlation between the two waymarks input
	% Calculate the frequency response for the latest generated multipath scene
	freqRespWaymarkB		= field.amp * exp(-1i*2*pi* (field.delay-fieldPropDelayFirst).'*(freqSearch));		% Frequency response after subtracting the common propagation time
	
	% If this is the initial run then there is nothing to compare to so save the calculated frequency response and return, marking the delay as zero.
	if (waymarkInit)
		waymarkDelay		= 0;
		freqRespWaymarkA	= freqRespWaymarkB;	% Copy current frequency response for next waymark calculation
		return;
	end


	% Calculate normaising magnitudes
	freqRespWaymarkANorm	= norm(freqRespWaymarkA);	
	freqRespWaymarkBNorm	= norm(freqRespWaymarkB);

	% Calculate the cost function for the initial delays
	costFunc				= freqRespWaymarkB .*conj(freqRespWaymarkA);					% Multiply element by element the frequency component with conjugate
	jDelta				= (abs(costFunc * exp(-1i*2*pi*(freqSearch).'*delta)))/(freqRespWaymarkBNorm*freqRespWaymarkANorm);	% Generate a delay for each frequency, apply it to the pressure magnitude element and sum
	[jBest, maxPos]	= max(jDelta);															% Find the best match
				
					% == Plot ==
					if (plotSearch)
						deltaPointsDelta	= delta;
						deltaPointsJDelta = jDelta;
					end
					
	% Evaluate the extreames of the position first to prevent an out of bounds error
	if (maxPos == 1)											% Advancing has the better correlation
		searchDelta = delta([maxPos,maxPos+1]);
	elseif (maxPos == length(delta))						% Delaying has better correlation
		searchDelta = delta([maxPos,maxPos-1]);
	% Evaluate comparision within the array
	elseif (jDelta(maxPos+1) > jDelta(maxPos-1))		% Advancing has the better correlation
		searchDelta = delta([maxPos,maxPos+1]);
	elseif (jDelta(maxPos+1) <= jDelta(maxPos-1))	% Delaying has better correlation
		searchDelta = delta([maxPos,maxPos-1]);	
	end



	% Itterate NUM_ITER times divideing delta by 2 (finding the mean)
	for it = 1:NUM_ITER
		jDelta = (abs(costFunc  *exp(-1i*2*pi*(freqSearch).'*mean(searchDelta))))/(freqRespWaymarkBNorm*freqRespWaymarkANorm);	% Evaluate the result for new delta

					% == Plot ==
					if (plotSearch)
						% Insert the delta datapoint into the array
						rangeBelow = (deltaPointsDelta <= mean(searchDelta));
						deltaPointsDelta	= [deltaPointsDelta(rangeBelow), mean(searchDelta),	deltaPointsDelta(~rangeBelow)];
						deltaPointsJDelta = [deltaPointsJDelta(rangeBelow), jDelta,		deltaPointsJDelta(~rangeBelow)];
					end

		if (jDelta >= jBest)													% Test if the result is an improvement
			jBest = jDelta;
			searchDelta = [mean(searchDelta) searchDelta(1)];		% If it is then put the new calculation into the top slot and move the previous best down
		else
			searchDelta(2) = mean(searchDelta);							% If not then replace the second best result
		end

	end    

					% == Plot ==
					if (plotSearch)
						plotFreqFig = figure('name', 'Search Freq. Responses');
						plot(abs(freqRespWaymarkA), 'b');
						hold on
						grid on	
						plot(abs(freqRespWaymarkB), 'r');
						title('Search Freq. Responses');
						ylabel('Magnitude')
						xlabel('Frequency')
						
						plotSearchFig = figure('name', 'Delay estimation');
						plot(deltaPointsDelta, deltaPointsJDelta, 'g-+');
						title('Delay estimation');
						ylabel('Estimation quality')
						xlabel('Delta delay')
						%pause;
					end
	
	waymarkDelay		= -1*searchDelta(1);								% Output the results delay shift between input waymarks
	freqRespWaymarkA	= freqRespWaymarkB;								% Copy current frequency response for next waymark calculation
end

