/*

   Module: 

      adapt.c

   Purpose: 

      Simulation of microphone response to moving point source in
      rectangular room.  Using iterative approach: delays at time n
      are obtained from delays at time n-1 with one Newton-Raphson
      iteration.

   Date: 

      25 February 1992

   Authors: 

      B. Champagne

   Affiliation: 

      INRS-Telecommunications
      Universite du Quebec
      Verdun, Quebec
      Canada  H3E 1H6

   Using the program:

      Main call:

         adapt audfilein coeff_file audfileout

      where

         audfilein:  input file containing source signal
         audfileout: output file for storing microphone response
         coeff_file: file containing the coefficients of the symmetrical
                     interpolation filter (the impulse response of this
                     filter is assumed symmetrical, so that only the
                     coefficients corresponding to the causal part
                     of the response are stored)

      Changing room characteristics, microphone location and source 
      trajectory:

         Room characteristics are defined with symbolic constants at the
         beginning of the program.  They can be changed easily.

         The microphone location and the source trajectory are defined
         in a function called calc_gr_gdotr.  The trajectory can be 
         changed to accommodate different scenarios.

   External routines required: 

      None.

*/
 

/* Standard libraries: */
#include <stdio.h>
#include <math.h>
#include <time.h>

/* Room dimensions in meters: */
#define Lx 10.0
#define Ly 10.0
#define Lz 3.0

/* Reflection coefficients: */
#define BETAXP 0.7   /* wall perpendicular to +x */
#define BETAXM 0.7   /* wall perpendicular to -x */
#define BETAYP 0.7
#define BETAYM 0.7
#define BETAZP 0.5
#define BETAZM 0.5

/* Speed of sound in air (m/s): */
#define C 343.0

/* Scaling factor for microphone output: */
#define AMP 200.0

/* Parameters related to interpolation process: */
#define FS      10000.0  /* sampling frequency of input data */
#define L       8        /* upsampling factor */
#define FSL     FS*L     /* interpolation rate */
#define NCOEF   59       /* interpolation filter has 2*NCOEF+1 taps */
#define SIZE    (2800*L) /* size of circular buffer for interpolated samples */
/* Note: this buffer must be large enough to store all past samples
   that enter the microphone response at a given time.  Knowledge of
   the reverberation time, or some other bound on the maximum allowed
   delay, is necessary to determine SIZE. */
#define K       (NCOEF+L-1)        /* NCOEF+L-1 */

/* Number of Newton-Raphson iterations: */
#define NITER  1

/* Macro substitution: */
#define MACFOR \
   for (r1 = -n1; r1 < n1+1; r1++) \
   for (r2 = -n2; r2 < n2+1; r2++) \
   for (r3 = -n3; r3 < n3+1; r3++) { \
   tmp1 = r1*Lx; \
   tmp2 = r2*Ly; \
   tmp3 = r3*Lz; \
   tmp4 = tmp1*tmp1 + tmp2*tmp2 + tmp3*tmp3; \
   if (tmp4 <= revrad2) {

/* External variables: */
char audfilein[256],    /* name of input audio file */
     audfileout[256],   /* name of output audio file */
     coeff_file[256],   /* name of file containing filter coefficients */
     line[80];          /* line buffer for file access */

int next,         /* index of next speech sample to be read */
    r1,r2,r3,     /* indices of images */
    n1,n2,n3,     /* limits of indices */
    i0buf2;       /* index pointing to most recent element of buf2 */

float speechin[40000],       /* input speech samples */
      speechout[40000],      /* output speech samples */
      tau[20][20][60],       /* propagation delay for source images*/
      taudot[20][20][60],    /* delay rates */
      intfilter[NCOEF+1],    /* interpolation filter coefficients */
      buf1[2*K+1],           /* buffer containing zero-padded signal */
      buf2[SIZE],            /* circular buffer for interpolated samples */
      revrad2,               /* square of reverberation radius */
      tmp1,tmp2,tmp3,tmp4;   /* temporary variables */

double beta[20][20][60];   /* composite reflection coefficients */
      
void calc_room_params(),
     calc_beta(),
     calc_gr_gdotr(float time, float *grpt, float *gdotrpt),
     read_interpolate(),
     read_filter_coeffs();

FILE *filein,
     *fileout;


/* Main function: */
main(int argc, char *argv[])
{
   int index,          /* position of current sample */
       nsamp,          /* number of samples in audio file */
       tauri,          /* interpolated delay expressed in high rate samples */
       nmax,           /* number of samples to be processed */
       i,j,k;          /* dummy variables */

   float scale,        /* scaling factor for output speech samples */
         time,         /* time in seconds */
         gr,           /* distance function */
         gdotr,        /* derivative of distance function */
         taur,         /* delay for current mode */
         taudotr,      /* delay rate */
         pi;           /* 3.14... */

   clock_t t1,t2,t3;   /* used for timing */

   switch (argc) {
   case 4:
      strcpy(audfilein,argv[1]);       /* input audio file name */
      strcpy(coeff_file,argv[2]);      /* file of filter coefficients */
      strcpy(audfileout,argv[3]);      /* output audio file name */
      break;
   case 3:
   case 2:
      printf("missing argument\n");
      exit(-1);
   case 1:
      printf("This program calculates the response of a microphone\n");
      printf("to a moving point source in a rectangular room\n"); 
      exit(0);
   } /* end switch */

/* Open output file: */
   fileout = fopen(audfileout, "w");

/* Preliminary calculations: */
   t1 = clock()/CLOCKS_PER_SEC;
   pi = 4. * atan(1.0);
   scale = AMP / (4.*pi*C);
   
/* Calculate reverberation time and related parameters: */
   calc_room_params();

/* Calculate composite reflection coefficients: */
   calc_beta();

/* Read coefficients of interpolation filter: */
   read_filter_coeffs();

/* Open, read and close input speech file: */
   filein = fopen(audfilein, "r");
   nsamp = 0;
   while (fgets(line, 80, filein) != NULL) {
      sscanf(line, "%f", &speechin[nsamp]);
      nsamp++;
   }
/* for (i=0; i<40; i++) printf("%16.7e \n", speechin[i]); */

/* Initialize zero-padded speech buffer (this buffer will be used to resample
   the input signal at the higher rate FSL: the zero-padded samples in this 
   buffer will be convolved with the interpolation filter coefficients): */
   next = 0;
   for (i=0; i<K; i++)
      buf1[i] = 0.0;
   for (i=K; i<2*K+1; i+=L) {
      buf1[i] = speechin[next++];
      for (j=i+1; j<i+L && j<2*K+1; j++)
         buf1[j] = 0.0;
   }
/*   for (i=0; i<2*K+1; i++) printf("%d %f\n", i-K, buf1[i]); */
   
/* Initialize high rate speech buffer: (this buffer is used to store
   past high-rate samples of the input speech signal, obtained by
   convolving the zero-padded buffer with the interpolation filter): */
   i0buf2 = 0;
   tmp1 = intfilter[0]*buf1[K];
   for (i=1; i<=NCOEF; i++)
      tmp1 += intfilter[i]*(buf1[K-i] + buf1[K+i]);
   buf2[0] = tmp1;
   for (i=1; i<SIZE; i++)
      buf2[i] = 0.0;

/* Main loop over samples: */
   t2 = clock()/CLOCKS_PER_SEC;
   nmax = nsamp - (1 + K/L);
   nmax = 4000;
   fprintf(fileout, "Output samples:\n");
   fprintf(fileout, "\n");
   for (index=0; index < nmax; index++) {

/*    Calculate delays and delay rates: */
      time = (float) index / (float) FS;   /* time in seconds */ 
      MACFOR
         taur = tau[r1+n1][r2+n2][r3+n3];
         for (k = 0; k < NITER; k++) { 
            calc_gr_gdotr(time-taur, &gr, &gdotr);
            taur = (gr + gdotr * taur) / (C + gdotr);
         }
         tau[r1+n1][r2+n2][r3+n3] = taur; 
         taudot[r1+n1][r2+n2][r3+n3] = gdotr/(C + gdotr);
         /* Alternative:
            taudot[] = (taur - tau[]) * (float) FS;
            tau[] = taur; */
      }}

/*    Calculate microphone output signal: */
      speechout[index] = 0.0;
      MACFOR
         /* Get delay: */
         taur = tau[r1+n1][r2+n2][r3+n3];

         /* Calculate attenuation factor: */
         taudotr = taudot[r1+n1][r2+n2][r3+n3];
         tmp1 = beta[r1+n1][r2+n2][r3+n3] * (1. - taudotr) / taur;

         /* Find value of input signal corresponding to delay taur 
         (linear interpolation between high-rate samples is used to 
         get improved estimate): */
         taur *= (float) FSL;
         tauri = taur;
         tmp3 = buf2[(i0buf2+tauri+1)%SIZE] - buf2[(i0buf2+tauri)%SIZE];
         tmp2 = buf2[(i0buf2+tauri)%SIZE] + tmp3 * (taur - (float) tauri);

         /* Add response from image n1,n2,n3 to microphone output: */
         speechout[index] += tmp1*tmp2;
      }}

      /* Scale microphone output: */
      speechout[index] *= scale;
      fprintf(fileout, "%d %16.7e\n", index, speechout[index]);

      /* Read next sample of source signal: */
      read_interpolate();
   }

   t3 = clock()/CLOCKS_PER_SEC;
   fprintf(fileout, "\n");
   fprintf(fileout, "CPU time spent in preliminary calculations: %d (sec)\n",
      t2-t1);
   fprintf(fileout, "\n");
   fprintf(fileout, "CPU time spent in main loop: %d (sec)\n", t3-t2);

/* Close output file: */
   fclose(fileout);

   return;
}  /* END main */


/* This function calculates various parameters related to the absorption
   properties of the room. */
void calc_room_params()
{
   float sx, sy, sz, vol;
   float alphaxp, alphaxm, alphayp, alphaym, alphazp, alphazm;
   float tap, revrad, revtime;

   fprintf(fileout, "Calculation of reverberation time:\n");
   fprintf(fileout, "\n");

   /* Areas of walls and volume of room: */
   sx = Ly * Lz;
   sy = Lz * Lx;
   sz = Lx * Ly;
   vol = Lx * Ly * Lz;
   fprintf(fileout, "   Sx = %f Sy = %f Sz = %f\n",sx,sy,sz);
   fprintf(fileout, "   Room volume = %f\n", vol);

   /* Absorption coefficients for walls: */
   alphaxp = 1. - BETAXP*BETAXP;  /* wall perpendicular to +x axis */
   alphaxm = 1. - BETAXM*BETAXM;  /* wall perpendicular to -x axis */
   alphayp = 1. - BETAYP*BETAYP;
   alphaym = 1. - BETAYM*BETAYM;
   alphazp = 1. - BETAZP*BETAZP;
   alphazm = 1. - BETAZM*BETAZM;
   fprintf(fileout, "   alphaxp = %f alphayp = %f alphazp = %f\n",
      alphaxp,alphayp,alphazp);

   /* Total absorptive power: */
   tap = (alphaxp + alphaxm)*sx + (alphayp + alphaym)*sy + 
      (alphazp + alphazm)*sz; 
   fprintf(fileout, "   Total absorptive power = %f\n", tap);

   /* Reverberation time: */
   revtime = 55.2 * vol/(tap * C);
   fprintf(fileout, "   Reverberation time = %f sec\n",revtime);

   /* Reverberation radius: */
   revrad = C * revtime;
   revrad2 = revrad * revrad;
   fprintf(fileout, "   Reverberation radius = %f\n",revrad);

   /* Calculate range of summations (we shall only retain images 
      in image rooms whose centers are within a distance revrad 
      from origin): */
   n1 = revrad/Lx; 
   n2 = revrad/Ly; 
   n3 = revrad/Lz; 
/*  n1 = n2 = n3 = 0; */
   fprintf(fileout, "   Range of sum: n1=%d, n2=%d, n3=%d\n",n1,n2,n3);
   fprintf(fileout, "\n");

   return;
}  /* END calc_room_params() */


/* This function calculates the composite reflection coefficients
   of all the images. */
void calc_beta()
{
   float nxp, nyp, nzp, nxm, nym, nzm;

   fprintf(fileout, "Calculation of composite reflection coefficients:\n");
   fprintf(fileout, "\n");

   MACFOR
      nxp = fabs(ceil(r1/2.));
      nyp = fabs(ceil(r2/2.));
      nzp = fabs(ceil(r3/2.));
      nxm = abs(r1) - nxp;
      nym = abs(r2) - nyp;
      nzm = abs(r3) - nzp;
      beta[r1+n1][r2+n2][r3+n3] = 
         pow(BETAXP,nxp) * pow(BETAXM,nxm) * pow(BETAYP,nyp) *
         pow(BETAYM,nym) * pow(BETAZP,nzp) * pow(BETAZM,nzm);
/*    printf("%d %d %d %f\n",r1,r2,r3,beta[r1+n1][r2+n2][r3+n3]);  */
   }}

   return;
} /* END calc_beta */


/* This function reads the coefficients of the interpolation filter. */
void read_filter_coeffs()
{
   int i;
   float sum;
   FILE *filepnt;

   fprintf(fileout, "Reading filter coefficients:\n");
   fprintf(fileout, "\n");

   /* Read coefficient file: */
   if ((filepnt = fopen(coeff_file,"r")) == NULL) {
      printf("Cannot open file of FIR interpolation filter coefficients\n"); 
      exit(0);
   } 
   for (i=0; i<=NCOEF; i++) { 
      fscanf(filepnt, "%f\n", &intfilter[i]); 
      /* printf("%d: %f\n", i, intfilter[i]); */
   }
   fclose(filepnt);

   /* Fourier transform of interpolation filter at zero frequency: */
   sum = intfilter[0];
   for (i=1; i<=NCOEF; i++) { 
      sum += 2*intfilter[i];   /* filter is symmetric */
   }
   fprintf(fileout, "   Amplitude of interpolation filter = %f\n", sum);
   fprintf(fileout, "\n");

   return;
} /* END */


/* This routine calculates the distance function gr(t) and its time 
   derivative gdotr(t) = d [gr(t)] / dt.  It can be modified to accommodate 
   different source trajectories and receiver coordinates. */
void calc_gr_gdotr(float time, float *grpt, float *gdotrpt)
{
   float xr, yr, zr,
         xs, ys, zs,
         vxs, vys, vzs,
         dx, dy, dz,
         eps1, eps2, eps3;

   /* Specify source trajectory (MKS system of units is used): */
   vxs = 0.0;
   vys = 3.0;   /* constant velocity of 3m/s along y-axis */
   vzs = 0.0;
   xs = 3.0;
   ys = -4.5 + vys*time;
   zs = 0.0;

   /* Specify receiver coordinates: */
   xr = 0.0;
   yr = 0.0;
   zr = 0.0;

   /* calculate gr: */
   eps1 = ((r1 % 2) != 0) ? -1. : 1.; 
   eps2 = ((r2 % 2) != 0) ? -1. : 1.; 
   eps3 = ((r3 % 2) != 0) ? -1. : 1.; 
   dx = r1*Lx + eps1*xs - xr; 
   dy = r2*Ly + eps2*ys - yr; 
   dz = r3*Lz + eps3*zs - zr;
   *grpt = sqrt(dx*dx + dy*dy + dz*dz);   

   /* calculate derivative of gr: */
   *gdotrpt = (eps1*vxs*dx + eps2*vys*dy + eps3*vzs*dz) / *grpt;

} /* END calc_gr_gdotr */


/* This function reads another speech sample and performs high rate
   interpolation. */
void read_interpolate()
{
   int i,j,k;
   float tmp;

   /* Shift zero-padded buffer by L samples and add new samples: */
   for (i=0; i<2*K+1-L; i++)
      buf1[i] = buf1[i+L];
   for (i=2*K+1-L; i<2*K+1; i++)
      if ((i-K)%L == 0)
         buf1[i] = speechin[next++];
      else
         buf1[i] = 0.0;

   /* Shift high rate speech buffer: */
   i0buf2 = (i0buf2 + SIZE - L)%SIZE;
   for (i=L-1; i>=0; i--) {
      tmp = intfilter[0]*buf1[K-i];
      for (j=1; j<=NCOEF; j++)
         tmp += intfilter[j]*(buf1[K-i-j] + buf1[K-i+j]);
      buf2[i0buf2+i] = tmp;
   }

   return;
} /* END read_interpolate */