function cwsonar(f0,fs)
% cwsonar(f0,fs)
% Coherent sonar with constant carrier (distance meter).
% Necessary hardware: Speaker connected to left audio output, (up to 20 kHz),
% microphone connected to left audio input (up to 20 kHz).
% At least one of both should be movable (no fixed distance).
% f0 : carrier frequency in Hz (Default: 17000)
% fs : sampling rate           (Default: 48000) 
% The figure shows the phase difference between received signal and 
% transmitted signal. One full turn of the pointer corresponds to a 
% relative motion of one wavelength between speaker and microphone. 
% The default wavelength is 2.0 cm (17000 Hz).
% Also the relative cumulative motion between speaker and microphone 
% is displayed. This value only is correct as long as the phase difference 
% between sequential blocks is absolutely not larger than pi, i.e. the 
% relative velocity must be lower than  0.166*fs/f0 in m/s . 
% Important: If the pointer rotates continuously at fixed position 
% of speaker and microphone then the sample rates of output and input 
% are not identical. This program then cannot be used.

% K. v. d. Heide,  e-mail:  v.d.heide@on-line.de

% Parameter-Defaults
% ==================
if nargin<2, fs = 48000; end              % sampling rate in samples per second
if nargin<1, f0 = 17000; end              % carrier frequency in Hz
 
% Constants
% =========
cs  = 340;                                % speed of sound in m/s
n   = 10;                                 % number of decimations by 1/2
blk = 2^n;                                % blockllength in samples
buffer = ceil(fs/blk);                    % output buffer in #blocks (ca. 1 second)
m   = round(blk*f0/fs);                   % #cycles per block
fc  = m*fs/blk;                           % true carrier frequency
txa = exp(-1i*2*pi*fc/fs*(1:blk)');       % analytic carrier within one block
txs = real(txa);                          % real signal within one block
q   = min(0.4,2*f0/fs);
gh  = 2*ceil(1/q);                        % necessary degree of Hilbert filter
bh  = firpm(gh,[q,1-q],[1 1],'Hilbert');  % design of Hilbertfilter

% Graphic
% =======
fh = figure;
set(fh,'Name','Distance Meter',...        % figure properties
       'NumberTitle','off',...
       'Units','centimeters',...
       'Position',[1 1 12 14],...
       'Color','w',...
       'Menubar','none',...
       'DockControls','off')
ah = axes;
set(ah,'Units','normalized',...           % axes properties
       'Position',[0 0 1 0.86],...
       'DataAspectRatio',[1 1 1],...
       'XLim',1.2*[-1 +1],...
       'YLim',1.2*[-1 +1])    
axis off
lh = line([0 0],[0 0],'Color','r','LineWidth',3);
text(0,1.45,['Distance Meter: ' num2str(100*cs/fc) ' cm per rotation'],...
            'HorizontalAlignment','center','FontSize',14)
text(0,1.3,'actual position:','HorizontalAlignment','right','FontSize',14)
th = text(0,1.3,'0.0000 m','HorizontalAlignment','left','FontSize',14);
line(cos(2*pi*(0:0.001:1)),sin(2*pi*(0:0.001:1)),'Color','k')
ds = ceil(100*cs/fc/32*10)/10;
dp = ds/(100*cs/fc)*2*pi;                 % angle step
ss = [1 1.05];
j  = 0;
for p=0:dp:2*pi
   line(ss*cos(p),ss*sin(p),'Color','k')
   if p<=pi/2 || p>=3/2*pi
      text(1.08*cos(p),1.08*sin(p),num2str(j*ds,'%3.1f'),'Color','k','Rotation',p*180/pi,...
           'HorizontalAlignment','left')
   else
      text(1.08*cos(p),1.08*sin(p),num2str(j*ds,'%3.1f'),'Color','k','Rotation',(p*180/pi-180),...
           'HorizontalAlignment','right')
   end
   j = j+1;
end

% Delete all running DAQs
% =======================
daqopen = daqfind;
for k=1:length(daqopen)
   stop(daqopen(k));
   delete(daqopen(k));
end

% Define Audio Channels
% =====================
ai = analoginput('winsound');
addchannel(ai,1);
ai.SampleRate    = fs;
ai.TriggerRepeat = inf;
ai.TriggerType   = 'immediate';
ai.SamplesPerTrigger = blk;
ai.TimeOut       = 1.0;
ao = analogoutput('winsound');
ch = addchannel(ao,1);
set(ao,'SampleRate',fs)
ao.TimeOut       = 1.0;
flushdata(ai)                             % delete old input data

% Start
% =====
x  = cell(n+1);                           % signal structure
xs = complex(zeros(1,n));                 % last sample
for k=1:n+1, x{k} = complex(zeros(blk*2^(k-1),1)); end
for k=1:buffer
   putdata(ao,txs);                       % output data into output buffer
end
start([ai ao])                            % start input and output
cnt = 0;                                  % block counter

% Period
% ======
while 1
   try
      rxs  = getdata(ai,blk);             % wait for blk input samples
      putdata(ao,txs);                    % output chirp data 
      rxa  = rxs + 1i*delay(filter(bh,1,rxs),-gh/2); % Hilbert filter
      x{1} = txa.*rxa;                    % spektral rotation into baseband
      for k=1:n                           % simple multirate lowpass filter
         x{k+1} = 0.5*x{k}(1:2:end-1) + 0.25*([xs(k); x{k}(2:2:end-2)] + x{k}(2:2:end));
         xs(k)  = x{k}(end);              % save last sample for next loop cycle
      end
      p   = angle(xs(n));                 % actual phase
      if cnt<3                            % 3 blocks to set the starting position 
         sp = 0;                          % start with zero phase sum
      else
         dp  = p - lp;                    % phase motion since last block
         if dp>pi                         % only -pi < dp < pi allowed
            dp = dp - 2*pi;
         elseif dp<-pi
            dp = dp + 2*pi;
         end
         sp  = sp + dp;                   % sum of phase motions since program start
      end
      d   = -sp*cs/(2*pi*fc);             % actual distance between speaker and microphone
      set(th,'String',[' ' num2str(d,' %8.3f m')]) % update distance
      lp  = p;                            % save actual phase
      amp = max(abs(x{n+1}));             % scaling factor
      set(lh,'XData',[0; real(x{n+1}/amp); 0],'YData',[0; imag(x{n+1}/amp); 0]) % update pointer
      drawnow
      cnt = cnt + 1;
      catch
         return                           % if any failure abort program
      end
end
