/* one_array_fasttde_dirloc_c.c
 *
 * MEX file: C implementation of the following MATLAB line:
 *
 *        [ az, el ] = one_array_fasttde_dirloc( block_frame_ind, sector_id );
 *
 % in "FASTTDE_detect_locate.m".
 *
 * Note that "ws" is a global variable.
 *
 * Our goal is to speedup things a bit.
 *
 * All inputs/outputs are done through the global variable "fasttde_wsp".
 */

#include <math.h>
#include <string.h>
#include "mex.h"

/* 1 or 0 */
#define VERBOSE             0

#if     VERBOSE == 1
#include "stdio.h"
#endif

/* Input Arguments */

#define  N_INPUTS        0

/* Output Arguments */

#define  N_OUTPUTS       0

/* All input/output are made through the "ws" global variable Note
 * that the outputs "ws.block.ssm_binary_decision" and
 * "ws.block.ssm_activeness" must be allocated by the user in advance.
 */

/* Misc */

#if !defined(MAX)
#define MAX(A, B)       ((A) > (B) ? (A) : (B))
#endif

#if !defined(MIN)
#define MIN(A, B)       ((A) < (B) ? (A) : (B))
#endif

#define PI 3.141592653589793115997963468544185161590576171875

#define TWOPI 6.28318530717958623199592693708837032318115234375

#define EPSILON 1e-100

/* Warning: due to an issue with fmod, you first need to make sure */
/* that all three angles A, AMIN and AMAX are positive values. */
#if !defined(ANGLEWITHIN)
#define ANGLEWITHIN( A, AMIN, AMAX )   ( ( 0 <= fmod( A - AMIN + EPSILON, TWOPI ) ) && ( fmod( A - AMIN + EPSILON, TWOPI ) < fmod( 2 * EPSILON + AMAX - AMIN, TWOPI ) ) )
#endif


#if VERBOSE == 1
/* ---------------------------------------------------------------------- */
/* debugging functions */

void print_double_array( double* doublePtr, unsigned int N, const char * a_string ) {

  unsigned int n;

  printf( "\n" );
  printf( a_string );
  printf( "[]: " );

  for( n = 0; n < N; n++ ) {
    printf( " [%d]:%g,", n, doublePtr[n] );
  }

  printf( "\n" );
  
}

void print_int_array( int* usPtr, unsigned int N, const char * a_string ) {

  unsigned int n;
  int * usmax;

  printf( "\n" );
  printf( a_string );
  printf( "[]: " );

  for( n = 0; n < N; n++ ) {
    printf( " [%d]:%d,", n, usPtr[n] );
  }

  printf( "\n" );
  
}

void print_unsigned_short_array( unsigned short* usPtr, unsigned int N, const char * a_string ) {

  unsigned int n;
  unsigned short * usmax;

  printf( "\n" );
  printf( a_string );
  printf( "[]: " );

  for( n = 0; n < N; n++ ) {
    printf( " [%d]:%d,", n, usPtr[n] );
  }

  printf( "\n" );
  
}

void print_unsigned_char_array( unsigned char* usPtr, unsigned int N, const char * a_string ) {

  unsigned int n;
  unsigned char * usmax;

  printf( "\n" );
  printf( a_string );
  printf( "[]: " );

  for( n = 0; n < N; n++ ) {
    printf( " [%d]:%d,", n, usPtr[n] );
  }

  printf( "\n" );
  
}
#endif


/* ----------------------------------------------------------------------
 * Wrapper: this is the gateway function to MATLAB
 * ---------------------------------------------------------------------- */

void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray*prhs[] )

{

  mxArray   *ws_mx, *iipl_mx, *ssPl_mx, *urz_mx, *uf_mx, *ugcc_mx, *maxd_mx, *uimin_mx, *uimax_mx, *azmin_mx, *azmax_mx, *elmin_mx, *elmax_mx, *azout_mx, *elout_mx, *an_mx, *success_mx;

  unsigned short *iipl, urz, uf, *uimin, *uimax;

  double *ugcc, *maxd, azmin, azmax, elmin, elmax, **ssPl;

  int nsubarray, n_sample, n_pair;

  int ndims;
  const int *dims;

  int   current_tdoa, current_max_tdoa, new_tdoa;
  double current_max, new_value, *ugcc_iter, *ugcc_iter_max, *norm_pair_tdoa;

  unsigned short *iipl_iter;

  double sub_az, sub_el, *Pmat, cos_alpha_j, cos_beta_j, cos_gamma_j, az, el;
  double az_real, az_imag, el_real, el_imag;

  int a, b, j, m, n, sub_n;
  double aa, bb, cc, dd, rr, xx, yy, zz;

  char a_str[1000];
  
  /* -------------------------------------------------- */
  /* ( 1 ) Get access to the global workspace */
  /* We assume that the output "ws.block.observed_gccphat" is already allocated
   */

  /* ws: global fasttde_wsp */

  ws_mx = ( mxArray * ) mexGetVariablePtr( "global", "fasttde_wsp" );
  if ( ws_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to the global variable 'fasttde_wsp'." );
  }
  if ( !mxIsStruct( ws_mx ) ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: the global variable 'fasttde_wsp' must be a structure." );
  }

  /* Now we can look for the fields of "ws" that we are interested in (first the inputs, then the outputs). */  

  /* ssPl: fasttde_wsp.square_subarray_Pmat_list */

  ssPl_mx = ( mxArray * ) mxGetField( ws_mx, 0, "square_subarray_Pmat_list" );
  if ( ssPl_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.square_subarray_Pmat_list'." );
  }
  if ( !mxIsCell( ssPl_mx ) ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: the global variable 'fasttde_wsp.square_subarray_Pmat_list' must be a cell array." );
  }
  nsubarray = mxGetNumberOfElements( ssPl_mx );
  
  ssPl = ( double** ) malloc( nsubarray * sizeof( double* ) );

  for ( a = 0; a < nsubarray; a++ ) {
    
    an_mx = mxGetCell( ssPl_mx, a );
    
    if ( an_mx == NULL ) {
      printf( "\n%d\n", a );
      mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not access an element of the global cell array 'fasttde_wsp.square_subarray_Pmat_list'." );
    }
    if ( mxGetClassID( an_mx ) != mxDOUBLE_CLASS ) {
      printf( "\n%d\n", a );
      mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: the global cell array 'fasttde_wsp.square_subarray_Pmat_list' must contain only matrices of doubles." );
    }
    m = mxGetM( an_mx );  n = mxGetN( an_mx );
    if ( ( m != 4 ) || ( n != 4 ) ) {
      printf( "\n%d\n", a );
      mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: the global cell array 'fasttde_wsp.square_subarray_Pmat_list' must contain only 4x4 matrices of doubles." );
    }
    
    /* Get the homogeneous transformation (rotation + translation) for this subarray. */
    
    ssPl[ a ] = mxGetPr( an_mx );
    
  }

  /* iipl: fasttde_wsp.i_i_pair_list */

  iipl_mx = ( mxArray * ) mxGetField( ws_mx, 0, "i_i_pair_list" );
  if ( iipl_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.i_i_pair_list'." );
  }
  if ( mxGetClassID( iipl_mx ) != mxUINT16_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: the global variable 'fasttde_wsp.i_i_pair_list' must be an array of uint16." );
  }
  ndims = mxGetNumberOfDimensions( iipl_mx );
  if ( ndims != 2 ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.i_i_pair_list' must be a 2-D array." );
  }
  m = mxGetM( iipl_mx ); n = mxGetN( iipl_mx );
  if ( ( m != 2 ) || ( n != nsubarray ) ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.i_i_pair_list' must be a ( 2 X nsubarray ) array." );
  }
  iipl = ( unsigned short * ) mxGetData( iipl_mx );

  /* urz: fasttde_wsp.up_rowzero */
  
  urz_mx = ( mxArray * ) mxGetField( ws_mx, 0, "up_rowzero" );
  if ( urz_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.up_rowzero'." );
  }
  if ( mxGetClassID( urz_mx ) != mxUINT16_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_rowzero' must be a uint16." );
  }
  urz     = ( unsigned short ) mxGetScalar( urz_mx );
  
  /* uf: fasttde_wsp.upfactor */
  
  uf_mx = ( mxArray * ) mxGetField( ws_mx, 0, "upfactor" );
  if ( uf_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.upfactor'." );
  }
  if ( mxGetClassID( uf_mx ) != mxUINT16_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.upfactor' must be a uint16." );
  }
  uf     = ( unsigned short ) mxGetScalar( uf_mx );
  
  /* ugcc: fasttde_wsp.up_gccphat (selected part of the upsampled time-domain GCC-PHAT) */

  ugcc_mx = ( mxArray * ) mxGetField( ws_mx, 0, "up_gccphat" );
  if ( ugcc_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.up_gccphat'." );
  }
  if ( mxGetClassID( ugcc_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_gccphat' must be an array of double." );
  }
  if ( mxIsComplex( ugcc_mx ) ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_gccphat' must be an array of real double (not complex)." );
  }
  ndims = mxGetNumberOfDimensions( ugcc_mx );
  if ( ndims != 2 ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_gccphat' must be a 2-D array." );
  }
  dims         = mxGetDimensions( ugcc_mx );
  n_sample     = dims[ 0 ];
  n_pair       = dims[ 1 ];
  
  ugcc = mxGetPr( ugcc_mx );

  /* maxd: fasttde_wsp.maxdelay */

  maxd_mx = ( mxArray * ) mxGetField( ws_mx, 0, "maxdelay" );
  if ( maxd_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.maxdelay'." );
  }
  if ( mxGetClassID( maxd_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.maxdelay' must be an array of double." );
  }
  if ( mxGetNumberOfElements( maxd_mx ) != n_pair ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.maxdelay' must be an array of 'n_pair' elements." );
  }
  maxd = ( double* ) mxGetData( maxd_mx );
  
  /* uimin: fasttde_wsp.up_index_min */

  uimin_mx = ( mxArray * ) mxGetField( ws_mx, 0, "up_index_min" );
  if ( uimin_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.up_index_min'." );
  }
  if ( mxGetClassID( uimin_mx ) != mxUINT16_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_index_min' must be an array of uint16." );
  }
  if ( mxGetNumberOfElements( uimin_mx ) != n_pair ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_index_min' must be an array of 'n_pair' elements." );
  }
  uimin = ( unsigned short* ) mxGetData( uimin_mx );

  /* uimax: fasttde_wsp.up_index_max */

  uimax_mx = ( mxArray * ) mxGetField( ws_mx, 0, "up_index_max" );
  if ( uimax_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.up_index_max'." );
  }
  if ( mxGetClassID( uimax_mx ) != mxUINT16_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_index_max' must be an array of uint16." );
  }
  if ( mxGetNumberOfElements( uimax_mx ) != n_pair ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.up_index_max' must be an array of 'n_pair' elements." );
  }
  uimax = ( unsigned short* ) mxGetData( uimax_mx );
  
  /* azmin: fasttde_wsp.az_min */
  
  azmin_mx = ( mxArray * ) mxGetField( ws_mx, 0, "az_min" );
  if ( azmin_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.az_min'." );
  }
  if ( mxGetClassID( azmin_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.az_min' must be a double." );
  }
  azmin = fmod( mxGetScalar( azmin_mx ), 2 * PI );
  
  /* azmax: fasttde_wsp.az_max */

  azmax_mx = ( mxArray * ) mxGetField( ws_mx, 0, "az_max" );
  if ( azmax_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.az_max'." );
  }
  if ( mxGetClassID( azmax_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.az_max' must be a double." );
  }
  azmax = mxGetScalar( azmax_mx );

  /* elmin: fasttde_wsp.el_min */

  elmin_mx = ( mxArray * ) mxGetField( ws_mx, 0, "el_min" );
  if ( elmin_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.el_min'." );
  }
  if ( mxGetClassID( elmin_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.el_min' must be a double." );
  }
  elmin = mxGetScalar( elmin_mx );
  
  /* elmax: fasttde_wsp.el_max */

  elmax_mx = ( mxArray * ) mxGetField( ws_mx, 0, "el_max" );
  if ( elmax_mx == NULL ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: could not get a pointer to 'fasttde_wsp.el_max'." );
  }
  if ( mxGetClassID( elmax_mx ) != mxDOUBLE_CLASS ) {
    mexErrMsgTxt( "one_array_fasttde_dirloc_c.c: 'fasttde_wsp.el_max' must be a double." );
  }
  elmax = mxGetScalar( elmax_mx );
    
  /* --- */

  /* Now check for the outputs (the two field are supposed to already exist */

  azout_mx = ( mxArray * ) mxGetField( ws_mx, 0, "az" );

  elout_mx = ( mxArray * ) mxGetField( ws_mx, 0, "el" );

  success_mx = ( mxArray * ) mxGetField( ws_mx, 0, "success" );
  

#if VERBOSE == 1
  printf( "one_array_fasttde_dirloc_c.c: nsubarray:%d, n_sample:%d, n_pair:%d, urz:%d, urf:%d\n", nsubarray, n_sample, n_pair, urz, uf );

  for( a = 0; a < nsubarray; a++ ) {

    sprintf( a_str, "ssPl[%d]", a );
    print_double_array( ssPl[ a ], 16, a_str );

  }

  print_double_array( ugcc, n_sample * n_pair, "ugcc" );

  print_double_array( maxd, n_pair, "maxd" );

  print_unsigned_short_array( uimin, n_pair, "uimin" );
  print_unsigned_short_array( uimax, n_pair, "uimax" );

  print_unsigned_short_array( iipl, 2 * nsubarray, "iipl" );
    
  printf( "one_array_fasttde_dirloc_c.c: azmin:%f, azmax:%f, elmin:%f, elmax:%f\n", azmin, azmax, elmin, elmax );
#endif

  /* =================================================== */
  /* ( 1 ) Time-Delay Estimation (TDE) within the bounds */
  /* of this sector */

  /* NOTE: all delays are expressed in terms of samples */
  /* (before upsampling) */
  
  norm_pair_tdoa = ( double * ) malloc( n_pair * sizeof( double ) );

  for ( a = 0; a < n_pair; a++ ) {

    /* Scan permissible delays for this sector and this pair of microphones */
    
    ugcc_iter     = ugcc + n_sample * a;
    ugcc_iter_max = ugcc_iter + uimax[ a ];
 
    ugcc_iter     += uimin[ a ] - 1;
    current_tdoa   = uimin[ a ];

    current_max_tdoa = current_tdoa++;
    current_max      = *ugcc_iter++;
    
    while ( ugcc_iter < ugcc_iter_max ) {
      
      new_value = *ugcc_iter++;
      new_tdoa  = current_tdoa++;
      
      if ( new_value > current_max ) {
	current_max      = new_value;
	current_max_tdoa = new_tdoa;
      }
      
    } 

    /* Store the maximum TDOA, normalized by the maximum delay */

    /* -> value between -1 and +1 */
    
    norm_pair_tdoa[ a ] = ( urz - current_max_tdoa ) / ( uf * maxd[ a ] );

  }
  
#if VERBOSE == 1
  print_double_array( norm_pair_tdoa, n_pair, "norm_pair_tdoa" );
#endif


  /* ==================================================== */
  /* ( 2 ) For each subarray, derive a direction estimate */
  /* from "norm_pair_tdoa" */

  /* Number of correct directions found so far (at most "nsubarray") */
  sub_n  = 0;
  
  /* azimuth: average all correct directions, as vectors in the complex plane */
  az_real = 0;
  az_imag = 0;
  
  /* elevation: average all correct directions, as vectors in the complex plane */
  el_real = 0;
  el_imag = 0;
  
  /* Start looping through subarrays */
  
  iipl_iter = iipl;    		/* pair index */
  
  for ( j = 0; j < nsubarray; j++ ) {

#if VERBOSE == 1
    printf( "\nsubarray #%d\n", j );
#endif

    /* retrieve Pmat, which defines the local coordinates */

    Pmat = ssPl[ j ];
    
#if VERBOSE == 1
    printf( "iipl: %d %d\n", *iipl, *(iipl+1) );
#endif

    /* ( 2.1 ) a direction vector in the local coordinate system */
    /* ( the 3 cosines in Brandstein's thesis Section 7.2 ) */
    
    cos_alpha_j = norm_pair_tdoa[ ( *iipl_iter++ ) - 1 ];
    cos_beta_j  = norm_pair_tdoa[ ( *iipl_iter++ ) - 1 ];
    
    aa          = 1 - cos_alpha_j * cos_alpha_j - cos_beta_j * cos_beta_j;

    if ( aa >= 0 ) {

      /* Yes the equation system is solvable -> we have a new direction */

      /* It may still be incorrect (e.g. out of the sector) */
      
      cos_gamma_j = sqrt( aa );
      
#if VERBOSE == 1
      printf( "cos_alpha_j:%f  cos_beta_j:%f  cos_gamma_j:%f\n", cos_alpha_j, cos_beta_j, cos_gamma_j );
#endif

      /* ( 2.2 ) Move back to the global coordinate system */
      
      xx = Pmat[ 0 ] * cos_alpha_j + Pmat[ 4 ] * cos_beta_j + Pmat[  8 ] * cos_gamma_j + Pmat[ 12 ];
      yy = Pmat[ 1 ] * cos_alpha_j + Pmat[ 5 ] * cos_beta_j + Pmat[  9 ] * cos_gamma_j + Pmat[ 13 ];
      zz = Pmat[ 2 ] * cos_alpha_j + Pmat[ 6 ] * cos_beta_j + Pmat[ 10 ] * cos_gamma_j + Pmat[ 14 ];
      
      rr =  sqrt( MAX( 0, xx * xx + yy * yy ) );
      
#if VERBOSE == 1
      printf( "xx:%f  yy:%f  zz:%f  rr:%f\n", xx, yy, zz, rr );
#endif
      
      /* ( 2.3 ) Transform into spherical coordinates (radius unknown) */
      /* in order to check whether it is within the current sector */

      sub_az = TWOPI + atan2( yy, xx ); /* make sure it is >= 0  (for fmod) */
      sub_el = TWOPI + atan2( zz, rr );	/* idem */

#if VERBOSE == 1
      printf( "sub_az: %f (%d),   sub_el: %f (%d)\n", sub_az, ANGLEWITHIN( sub_az, azmin, azmax ), sub_el, ANGLEWITHIN( sub_el, elmin, elmax ) );
      printf( "%f %f,   %f %f \n",  fmod( sub_az - azmin + EPSILON, TWOPI ), fmod( 2 * EPSILON + azmax - azmin, TWOPI ),  fmod( sub_el - elmin + EPSILON, TWOPI ), fmod( 2 * EPSILON + elmax - elmin, TWOPI )  );
#endif
      
      if ( ANGLEWITHIN( sub_az, azmin, azmax ) && ANGLEWITHIN( sub_el, elmin, elmax ) ) {

	/* Yes! The new direction estimate is within the sector */

	/* Update the sum in complex plane */

	az_real += xx / MAX( EPSILON, rr );
	az_imag += yy / MAX( EPSILON, rr );

	el_real += rr;
	el_imag += zz;
	
	/* Update the count of correct directions */
	sub_n++;
	
      }	/* If the direction estimate is within the sector */

    } /* If the equation system is solvable */
    
  } /* Loop through subarrays */

#if VERBOSE == 1
  printf( "\nsub_n: %d\n", sub_n );
#endif

  if ( sub_n > 0 ) {
    
    /* We found at least one correct direction. */
    /* Return the average direction */

    az = atan2( az_imag, az_real );
    el = atan2( el_imag, el_real );

    if ( success_mx != NULL ) {
      mxDestroyArray( success_mx );
    }
    mxSetField( ws_mx, 0, "success", mxCreateLogicalScalar( 1 ) );

    if ( azout_mx != NULL ) {
      mxDestroyArray( azout_mx );
    }
    mxSetField( ws_mx, 0, "az", mxCreateDoubleScalar( az ) );

    if ( elout_mx != NULL ) {
      mxDestroyArray( elout_mx );
    }
    mxSetField( ws_mx, 0, "el", mxCreateDoubleScalar( el ) );
    
#if VERBOSE == 1
    printf( "one_array_fasttde_dirloc_c.c: success:%d  az:%f  el:%f\n", 1, az, el );
#endif

  } else {

    if ( success_mx != NULL ) {
      mxDestroyArray( success_mx );
    }
    mxSetField( ws_mx, 0, "success", mxCreateLogicalScalar( 0 ) );

#if VERBOSE == 1
    printf( "one_array_fasttde_dirloc_c.c: success:%d\n", 0 );
#endif

  }
  
  /* =================================================== */
  /* ( 3 ) Keep only the directions estimates that are   */
  /* within the sector. Merge them if more than one left */

  /* =================================================== */
  /* Free some memory */

  free( ssPl );
  free( norm_pair_tdoa );
  
}