function out = theoretical_crosscorr_analysis( p )

% function out = theoretical_crosscorr_analysis( p )
% 
% Simulate the theoretical fullband and narrowband cross-correlation between lapel
% microphone and distant microphone, in the case of a spherical wave emitted by a
% point source and travelling through a free field.
% 
% We use the GCC-PHAT measure of cross-correlation (Knapp 1976).
%
% This requires a lapel WAV file, and a ground-truth file, as for
% exemple in the recordings "seq01-1p-0000" and "seq03-1p-0000" of the
% AV16.3 Corpus ( http://mmm.idiap.ch/av16.3_v6 ).
%
% Both input "p" and output "out" are structures.
%
% The fields of "p" are the parameters of this function.
% Mandatory parameters:
%
%   p.static_gt_filename        :   string
% 
%   p.lapelmic_filename         :   string
%
%   p.framelength_sec           :   duration of frame, in seconds
%
% Optional parameters:
%   -> check the beginning of this code ( dbtype crosscorr_analysis ).
%
% The output structure "out" includes:
%
%   out.p : the input parameters, including the WAV filenames
%   specified by the user, for the lapel and the distant microphones
%   (see above). ".p" also includes optional parameters filled with
%   default values.
%
% and a structure array "out.loc", with as many elements as speaker
% locations. Each element has the following fields:
%
%   out.loc( i_loc ).lapelmic : a structure giving information about
%   the part of the lapel signal that we used, for this speaker
%   location.
%
%   out.loc( i_loc ).lapelmic.seg_sample_mat : a 2 x N_seg matrix of
%   integers (sample indices in the WAV file). Each column gives the
%   beginning and the end of each of the N_seg speech segments, for
%   this speaker location.
%
%   out.loc( i_loc ).distantmic : a structure array of 1 element,
%   containing statistics for a simulated distant microphone,
%   corresponding to pure delay.
%
%   out.loc( i_loc ).distantmic.seg_sample_mat : a 2 x
%   N_seg matrix of integers (sample indices in the WAV file).
%
%   out.loc( i_loc ).distantmic.delay : a N_delay x 1
%   vector of delay values, in sampling periods, relative to the
%   theoretical time of flight the lapel and this distant microphone
%   (th. TOF = distance / speed of sound).
%   
%   -> the delay values are not necessarily integers because we use
%   upsampling.
%
%   -> each row of ".fullband_gcc" and ".narrowband_gcc" contains the GCC-PHAT
%   value(s) associated with the corresponding delay value in ".delay"
%
%   out.loc( i_loc ).distantmic.fullband_gcc : GCC-PHAT
%   function in the time-domain. Dimension 1 corresponds to the values
%   in ".delay".
%
%   out.loc( i_loc ).distantmic.fullband_gccmax : peak
%   value of the fullband GCC-PHAT function between the lapel and this
%   microphone, in the time domain.
%
%   out.loc( i_loc ).distantmic.fullband_delaymax : peak
%   delay of the fullband GCC-PHAT function between the lapel and this
%   microphone, in the time domain.
%
%   -> ".narrowband_gcc", ".narrowband_gccmax" and
%   ".narrowband_delaymax" contain the same type of result as
%   ".fullband_...", but for individual narrowbands (second dimension
%   of each matrix).
%
% By Guillaume Lathoud, 2006  (email address in "my_email_address.jpg")

  if nargin < 1
    error( [ mfilename ' needs at least one input argument.' ] );
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % ( 1 ) Get parameters from the user and the files
  
  % Check for mandatory input parameters

  required_param = { 'static_gt_filename', 'lapelmic_filename', 'framelength_sec' };
  check_param( required_param, fieldnames( p ) );
  
  % Fill in default values for optional parameters

  p_default = [];

  p_default.frameshift_sec  = p.framelength_sec / 2;
  p_default.preemp          = 0.97;
  p_default.do_zero_mean    = 1;
  
  p_default.f_low  = 50;  % Value in Hz
  p_default.f_high = +Inf; % Value in Hz, or +Inf for Nyquist frequency
  p_default.n_narrowband = 80; % Number of narrowbands between "f_low" and "f_high"

  % Upsampling factor for the cross-correlation
  p_default.upfactor = 20;
  p_default.filterorder = [];

  % Tell me whether I should save the cross-correlograms
  % (this takes a lot of space)
  p_default.save_gcc = 1;
  
  % To get a reasonable starting point for delay search,
  % we assume a rough speed of sound in the air
  p_default.c = 342;  % in m/s

  % Assume that the lapel is 15 cm away from the mouth
  p_default.lapelmic_d = 0.15;
  
  % Proportion of the signals that we'll use.
  % We drop the queue of each utterance,
  % since it is usually weaker than the rest, 
  % (at least in average),
  % and therefore more sensitive to reverberations.
  p_default.utterance_prop = 0.75;

  % To limit the memory usage,
  % we limit the range of delays
  % around the theoretical free-field delay
  p_default.delay_min = -75;   % in sampling periods
  p_default.delay_max = +125;   % in sampling periods
  
  % Verbosity
  p_default.verbose = 1;

  % Additional parameters
  p_default.filterorder     = [];  % see below
  p_default.filterhalforder = [];  % see below
  p_default.B               = [];  % see below

  p = fill_default( p, p_default );
  
  %%%

  if isempty( p.filterorder )
    p.filterorder = 6 * p.upfactor;
  end

  if mod( p.filterorder, 2 )
    error( [ mfilename ': p.filterorder must be an even number.' ] );
  end

  if isempty( p.filterhalforder )
    p.filterhalforder = p.filterorder / 2;
  end

  if isempty( p.B )
    p.B = p.upfactor * fir1( p.filterorder, 1 / p.upfactor );
  end
  
  % Get the sampling frequency
  
  [ tmp, p.fs ] = wavread( p.lapelmic_filename, 'size' );
  
  p.f_high = min( p.fs / 2, p.f_high );
  
  % Parameters for the comparison
  
  p.framelength = round( p.framelength_sec * p.fs );
  p.frameshift  = round( p.frameshift_sec * p.fs );
  
  %%%

  if p.verbose
    disp( ' ' );
    disp( [ mfilename ' parameters:' ] );
    disp( p );
  end

  %%%

  % Bands in which we'll split the signal
  
  p.narrowbandwidth  = ( p.f_high - p.f_low ) / p.n_narrowband;
  p.narrowbandmat    = p.f_low + [ 0:p.n_narrowband-1; 1:p.n_narrowband ] * p.narrowbandwidth;
  p.narrowbandcenter = mean( p.narrowbandmat, 1 );
  
  % Create the corresponding filters
  
  p.narrowbandfilter = repmat( NaN, p.filterorder + 1, p.n_narrowband );

  for a = 1:p.n_narrowband
    
    if abs( p.narrowbandmat( 1, a ) ) < eps
      p.narrowbandfilter( :, a ) = fir1( p.filterorder, p.narrowbandmat( 2, a ).' / p.fs * 2, 'DC-1' ).';
    elseif abs( p.narrowbandmat( 2, a ) - p.fs / 2 ) < eps
      p.narrowbandfilter( :, a ) = fir1( p.filterorder, p.narrowbandmat( 1, a ).' / p.fs * 2, 'DC-0' ).';
    else
      p.narrowbandfilter( :, a ) = fir1( p.filterorder, p.narrowbandmat( :, a ).' / p.fs * 2, 'DC-0' ).';
    end    

  end

  % Move to the frequency domain
  
  p.narrowbandH  = fft( p.narrowbandfilter, 2 * p.framelength, 1 );
  p.narrowbandH2 = real( p.narrowbandH .* conj( p.narrowbandH ) );  % Real to remove possible imaginary epsilons
  
  p.narrowbandH2_GCC = p.narrowbandH2 > 1e-3;  % Necessary to filter GCC-PHAT
  
  % Based on the 3D ground-truth, estimate the relative distance between
  % lapel locations and microphones in the array.
  
  [ p.d, p.static_gt, p.p3d, p.g ] = estimate_dist( p.static_gt_filename );

  p.n_loc = size( p.d, 2 );

  p.d_diff = p.d - p.lapelmic_d;
  
  p.s_diff = p.d_diff / p.c * p.fs;  % time delay between lapel location and array microphones, in samples

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % ( 2 ) Simulate a distant microphone

  n_dist_mic = 1;

  if p.verbose
    disp( ' ' );
    disp( sprintf( [ mfilename ': iterating through %d speaker locations.' ], p.n_loc ) );
    chrono = chrono_start( p.n_loc );
  end

  % Structure array where we'll store the output
  
  loc = [];
  
  for i_loc = 1:p.n_loc
    
    %%%%%%%%%%
    % ( 2.1 ) Define speech segments and get the lapel signal for this speaker location
    
    % Which speech segments
    
    i_seg = find( p.static_gt.pos_ind == i_loc );
    
    seg_sample_mat = zeros( 2, 0 );
    
    for qq = i_seg(:).'
      
      seg = p.static_gt.sp_seg( :, qq );
      seg = [ seg(1) ;  seg( 1 ) + p.utterance_prop * diff( seg ) ];
      seg_sample = round( seg(:) * p.fs );
      
      seg_sample_mat = [ seg_sample_mat  seg_sample ];
      
    end
    
    % Get the lapel signal
    
    lapelmic.seg_sample_mat = seg_sample_mat;
    lapelmic.S              = wavreadconcatspectrum( p.lapelmic_filename, seg_sample_mat, p );
    
    %%%%%%%%%%
    % ( 2.2 ) Simulate a distant microphone for a point source emitting a spherical wave in free field
    %         Fullband cross-correlation analysis between the lapel and each microphone.
    %         Narrowband cross-correlation analysis between the lapel and each microphone.
    
    distantmic = [];
    
    for zzz = 1:n_dist_mic
      
      i_mic       = zzz;
      wavfilename = [];

      % Store some info

      newmic.i_mic       = i_mic;
      newmic.wavfilename = wavfilename;

      % Get the microphone array signal --> THIS IS WHERE WE DO THE SIMULATION
      
      newmic.seg_sample_mat = lapelmic.seg_sample_mat;
      newmic.S              = wavreadconcatspectrum( p.lapelmic_filename, lapelmic.seg_sample_mat, p );
    
      % Compare the spectra
      
      % First, average the cross-correlation spectrum,
      % in the complex domain
      
      CC = mean( newmic.S .* conj( lapelmic.S ), 2 );
      
      % Second, PHAT weighting to resist more to reverberations
      
      GCC = CC ./ abs( eps + CC );
      
      % "_1" stands for "original time-domain GCC function" (1 element per sample = not upsampled yet)
      gcc_1 = real( fftshift( ifft( GCC ) ) );
      
      % Third, upsample the fullband GCC-PHAT in the time-domain
      gcc   = filter( p.B, 1, upsample( gcc_1, p.upfactor ) );
      gcc   = gcc( p.filterhalforder+1:end );
      newmic.delay = ( 1 + ( 0:numel( gcc )-1 ) / p.upfactor ) - ( p.framelength + 1 );
      
      % Fullband delay
      
      [ tmp, ind ]                = max( gcc );
      newmic.fullband_gccmax      = tmp;
      newmic.fullband_delaymax    = newmic.delay( ind );
      newmic.fullband_gcc         = [];
      if p.save_gcc
	newmic.fullband_gcc         = gcc;
      end
      
      % Narrowband delay
      
      newmic.narrowband_gccmax      = repmat( NaN, 1, p.n_narrowband );
      newmic.narrowband_delaymax    = repmat( NaN, 1, p.n_narrowband );
      newmic.narrowband_gcc         = [];
      if p.save_gcc
	newmic.narrowband_gcc         = repmat( NaN, numel( gcc ), p.n_narrowband );
      end

      for i_narrowband = 1:p.n_narrowband
	
	gcc_1 = real( fftshift( ifft( GCC .* p.narrowbandH2_GCC( :, i_narrowband ) ) ) );
      
	% Fourth, upsample the narrowband GCC-PHAT in the time-domain
	
	gcc = filter( p.B, 1, upsample( gcc_1, p.upfactor ) );
	gcc = gcc( p.filterhalforder+1:end );
	
	% Narrowband delay
	
	[ tmp, ind ]                         = max( gcc );
	newmic.narrowband_gccmax( i_narrowband )   = tmp;
	newmic.narrowband_delaymax( i_narrowband ) = newmic.delay( ind );
	if p.save_gcc
	  newmic.narrowband_gcc( :, i_narrowband )   = gcc;
	end
	
      end

      % Keep only a somewhat interesting part of the gcc
      % to save some memory.
      
      % Impose a user-defined limitation of the range of delays, to save memory
      
      ind = find( ( p.delay_min - eps < newmic.delay ) & ( newmic.delay < p.delay_max + eps ) );
      
      % Apply the limitation and save memory
      
      newmic.delay          = newmic.delay( ind );
      if p.save_gcc
	newmic.fullband_gcc   = newmic.fullband_gcc( ind );
	newmic.narrowband_gcc = newmic.narrowband_gcc( ind, : );
      end
      
      % Store it
      
      distantmic = cat( 1, distantmic, newmic );
      
    end  % loop through microphones

    % Remove spectra

    distantmic = rmfield( distantmic, 'S' );
    lapelmic   = rmfield( lapelmic, 'S' );
    
    % Save the result
    
    loc( i_loc ).lapelmic   = lapelmic;
    loc( i_loc ).distantmic = distantmic;

    % Verbosity
    
    if p.verbose
      chrono = chrono_check( chrono, i_loc );
    end
    
  end   % Of loop through speaker locations

  if p.verbose
    chrono_stop( chrono );
  end

  out = [];
  out.p = p;
  out.loc = loc;
  
  return;
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ d, static_gt, p3d, g ] = estimate_dist( static_gt_filename )

  % Load spatial and temporal ground-truth
  % ( one speaker at several locations )
  
  load( static_gt_filename, 'static_gt' );
  
  % Position of the lapel w.r.t. the microphone array center
  
  p3d = static_gt.array( 1 ).Pmat * static_gt.p3d;
  
  % Geometry of the array (copied from "seq_av163.m" in the AV16.3 directory)

  a = (4:11)*pi/4;
  g = 0.1* [cos(a);sin(a);0*a];
  
  % Estimate the distance between each microphone in the array and each lapel location

  n_loc = size( p3d, 2 );
  n_mic = size( g, 2 );

  d = repmat( NaN, n_mic, n_loc );

  for a = 1:n_mic

    delta = p3d( 1:3, : ) - repmat( g( :, a ), 1, n_loc );

    d( a, : ) = sqrt( sum( delta .^ 2, 1 ) );

  end
  



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ S, fs, nbits ] = wavreadconcatspectrum( lapelmic_filename, seg_sample_mat, p )

  if nargin < 3
    error( [ mfilename '.wavreadconcatspectrum() needs 3 input arguments.' ] );
  end

  S = zeros( 2 * p.framelength, 0 );
  
  for a = 1:size( seg_sample_mat, 2 )

    [ x, fs, nbits ] = wavread( lapelmic_filename, seg_sample_mat( :, a ).' );

    nx = numel( x );
    
    % Full frames only
    new_nx = p.framelength + floor( ( nx - p.framelength ) / p.frameshift ) * p.frameshift;

    x = x( 1:new_nx );

    S = [ S  my_spectrum( x, p.framelength, p.frameshift, p.preemp, p.do_zero_mean ) ];
  
  end
  