/*
 * FILE: resample.c
 *   BY: Julius Smith (at CCRMA, Stanford U)
 * C BY: translated from SAIL to C by Christopher Lee Fraley
 *          (cf0v@spice.cs.cmu.edu or @andrew.cmu.edu)
 * DATE: 7-JUN-88
 *
 * HACKED: Lance Norskog, Dec. 28, 1991, to make sealed resampler for SoundKit
 * 	usage: kaiser factor [ rolloff [ beta ] ]
 */

char resampleVERSION[] = "1.3  (24-JUN-88, 3:00pm)";

/*
 *      The original version of this program in SAIL may be found in:
 *      /../s/usr/mhs/resample  or  /../x/usr/rbd/src/resample
 *
 *      Implement sampling rate conversions by (almost) arbitrary factors.
 *      Based on SRCONV.SAI[SYS,MUS] with new algorithm by JOS&PG.
 *      The program internally uses 16-bit data and 16-bit filter
 *      coefficients.
 *
 *      Reference: "A Flexible Sampling-Rate Conversion Method,"
 *      J. O. Smith and P. Gossett, ICASSP, San Diego, 1984, Pgs 19.4.
 *
 *      * Need overflow detection in Filter() routines.  Also, we want
 *        to saturate instead of wrap-around and report number of clipped
 *        samples.
 */

/* CHANGES from original SAIL program:
 *
 * 1. LpScl is scaled by Factor (when Factor<1) in resample() so this is
 *       done whether the filter was loaded or created.
 * 2. makeFilter() - ImpD[] is created from Imp[] instead of ImpR[], to avoid
 *       problems with round-off errors.
 * 3. makeFilter() - ImpD[Nwing-1] gets NEGATIVE Imp[Nwing-1].
 * 4. SrcU/D() - Switched order of making guard bits (v>>Nhg) and
 *       normalizing.  This was done to prevent overflow.
 */

/* LIBRARIES needed:
 *
 * 1. filterkit
 *       readFilter() - reads standard filter file
 *       FilterUp()   - applies filter to sample when Factor >= 1
 *       FilterUD()   - applies filter to sample for any Factor
 *       Query()      - prompt user for y/n answer with help.
 *       GetDouble()  - prompt user for a double with help.
 *       GetUShort()  - prompt user for a UHWORD with help.
 *
 * 2. math
 */




#include <stdio.h>
#include <math.h>
#include <string.h>
#include "stdefs.h"
#include "filterkit.h"
#include "resample.h"
#include "protos.h"

#define IBUFFSIZE 1024                         /* Input buffer size */
#define OBUFFSIZE (IBUFFSIZE*MAXFACTOR+2)      /* Calc'd out buffer size */

double Beta = 2.120;

fail(s)
char *s;
{
   fprintf(stderr, "kaiser: %s\n\n", s);  /* Display error message  */
   exit(1);                        /* Exit, indicating error */
}

fails(s,s2)
char *s, *s2;
{
   fprintf(stderr, "kaiser: ");           /* Display error message  */
   fprintf(stderr, s,s2);
   fprintf(stderr, "\n\n");
   exit(1);                        /* Exit, indicating error */
}




/* Help Declarations */

/* Global file pointers: */
FILE *fin, *fout;

closeData()
{
   (void) fclose(fin);
   (void) fclose(fout);
}


int readData(Data, DataArraySize, Xoff)  /* return: 0 - notDone */
HWORD Data[];                            /*        >0 - index of last sample */
int DataArraySize, Xoff;
{
   int Nsamps, Scan;
   short val;
   HWORD *DataStart;

   DataStart = Data;
   Nsamps = DataArraySize - Xoff;   /* Calculate number of samples to get */
   Data += Xoff;                    /* Start at designated sample number */
   for (; Nsamps>0; Nsamps--)
      {
      Scan = fread(&val, 1, 2, fin);      /* Read in Nsamps samples */
      if (Scan==EOF || Scan==0)            /*   unless read error or EOF */
         break;                            /*   in which case we exit and */
      *Data++ = val;
      }
   if (Nsamps > 0)
      {
      val = Data - DataStart;              /* (Calc return value) */
      while (--Nsamps >= 0)                /*   fill unread spaces with 0's */
         *Data++ = 0;                      /*   and return FALSE */
      return(val);
      }
   return(0);
}



writeData(Nout, Data)
int Nout;
HWORD Data[];
{
   short s;
   /* Write Nout samples to ascii file */
   while (--Nout >= 0) {
      s = *Data++;
      fwrite(&s, 1, 2, fout);
   }
}




getparams(argc, argv, Factor, Froll, Beta)
int argc;
char **argv;
double *Factor, *Froll, *Beta;
{
   if ((argc < 2) || (argc > 4))
      fail("format is 'resample factor [ rolloff [ beta ] ]'");
   if ((*Factor = atof(argv[1])) < 0.01)
      fail("conversion factor no good");
   if (argc == 2)
      *Froll = 0.5;
   else if ((*Froll = atof(argv[2])) < 0.01)
      fail("rolloff factor no good");
   if (argc == 3)
      *Beta = 2.120;
   else if ((*Beta = atof(argv[3])) < 2.0)
      fail("Beta factor no good");
   fin = stdin;
   fout = stdout;

   /* Check for illegal constants */
   if (Np >= 16)
      fail("Error: Np>=16");
   if (Nb+Nhg+NLpScl >= 32)
      fail("Error: Nb+Nhg+NLpScl>=32");
   if (Nh+Nb > 32)
      fail("Error: Nh+Nb>32");
}



/* Sampling rate up-conversion only subroutine;
 * Slightly faster than down-conversion;
 */
SrcUp(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
HWORD X[], Y[], Imp[], ImpD[];
UHWORD Nx, Nwing, LpScl;
UWORD *Time;
double Factor;
BOOL Interp;
{
   HWORD *Xp, *Ystart;
   WORD v;

   double dt;                  /* Step through input signal */ 
   UWORD dtb;                  /* Fixed-point version of Dt */
   UWORD endTime;              /* When Time reaches EndTime, return to user */

   dt = 1.0/Factor;            /* Output sampling period */
   dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */

   Ystart = Y;
   endTime = *Time + (1<<Np)*(WORD)Nx;
   while (*Time < endTime)
      {
      Xp = &X[*Time>>Np];      /* Ptr to current input sample */
      v = FilterUp(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
         -1);                  /* Perform left-wing inner product */
      v += FilterUp(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
         1);                   /* Perform right-wing inner product */
      v >>= Nhg;               /* Make guard bits */
      v *= LpScl;              /* Normalize for unity filter gain */
      *Y++ = v>>NLpScl;        /* Deposit output */
      *Time += dtb;            /* Move to next sample by time increment */
      }
   return (Y - Ystart);        /* Return the number of output samples */
}



/* Sampling rate conversion subroutine */

SrcUD(X, Y, Factor, Time, Nx, Nwing, LpScl, Imp, ImpD, Interp)
HWORD X[], Y[], Imp[], ImpD[];
UHWORD Nx, Nwing, LpScl;
UWORD *Time;
double Factor;
BOOL Interp;
{
   HWORD *Xp, *Ystart;
   WORD v;

   double dh;                  /* Step through filter impulse response */
   double dt;                  /* Step through input signal */
   UWORD endTime;              /* When Time reaches EndTime, return to user */
   UWORD dhb, dtb;             /* Fixed-point versions of Dh,Dt */

   dt = 1.0/Factor;            /* Output sampling period */
   dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */

   dh = MIN(Npc, Factor*Npc);  /* Filter sampling period */
   dhb = dh*(1<<Na) + 0.5;     /* Fixed-point representation */

   Ystart = Y;
   endTime = *Time + (1<<Np)*(WORD)Nx;
   while (*Time < endTime)
      {
      Xp = &X[*Time>>Np];      /* Ptr to current input sample */
      v = FilterUD(Imp, ImpD, Nwing, Interp, Xp, (HWORD)(*Time&Pmask),
          -1, dhb);            /* Perform left-wing inner product */
      v += FilterUD(Imp, ImpD, Nwing, Interp, Xp+1, (HWORD)((-*Time)&Pmask),
           1, dhb);            /* Perform right-wing inner product */
      v >>= Nhg;               /* Make guard bits */
      v *= LpScl;              /* Normalize for unity filter gain */
      *Y++ = v>>NLpScl;        /* Deposit output */
      *Time += dtb;            /* Move to next sample by time increment */
      }
   return (Y - Ystart);        /* Return the number of output samples */
}



int resample(Imp, ImpD, LpScl, Nmult, Nwing, Factor, InterpFilt)
HWORD Imp[], ImpD[];
UHWORD LpScl, Nmult, Nwing;
double Factor;
BOOL InterpFilt;
{
   UWORD Time;                       /* Current time/pos in input sample */
   UHWORD Xp, Ncreep, Xoff, Xread;
   HWORD X[IBUFFSIZE], Y[OBUFFSIZE]; /* I/O buffers */
   UHWORD Nout, Nx;
   int i, Ycount, last;

   /* Account for increased filter gain when using Factors less than 1 */
   if (Factor < 1)
      LpScl = LpScl*Factor + 0.5;
   /* Calc reach of LP filter wing & give some creeping room */
   Xoff = ((Nmult+1)/2.0) * MAX(1.0,1.0/Factor) + 10;
   if (IBUFFSIZE < 2*Xoff)      /* Check input buffer size */
      fail("IBUFFSIZE (or Factor) is too small");
   Nx = IBUFFSIZE - 2*Xoff;     /* # of samples to proccess each iteration */

   last = FALSE;                /* Have not read last input sample yet */
   Ycount = 0;                  /* Current sample and length of output file */
   Xp = Xoff;                   /* Current "now"-sample pointer for input */
   Xread = Xoff;                /* Position in input array to read into */
   Time = (Xoff<<Np);           /* Current-time pointer for converter */

   for (i=0; i<Xoff; X[i++]=0); /* Need Xoff zeros at begining of sample */
    
   do {
      if (!last)                /* If haven't read last sample yet */
         {
         last = readData(X, IBUFFSIZE, (int)Xread); /* Fill input buffer */
         if (last && (last+Xoff<Nx))  /* If last sample has been read... */
            Nx = last + Xoff;   /* ...calc last sample affected by filter */
	 }
      if (Factor >= 1)          /* Resample stuff in input buffer */
         Ycount += Nout = SrcUp(X,Y,Factor,&Time,Nx,Nwing,LpScl,Imp,ImpD,
            InterpFilt);        /* SrcUp() is faster if we can use it */
      else
         Ycount += Nout = SrcUD(X,Y,Factor,&Time,Nx,Nwing,LpScl,Imp,ImpD,
            InterpFilt);
      Time -= (Nx<<Np);           /* Move converter Nx samples back in time */
      Xp += Nx;                   /* Advance by number of samples processed */
      Ncreep = (Time>>Np) - Xoff; /* Calc time accumulation in Time */
      if (Ncreep)
         {
         Time -= (Ncreep<<Np);    /* Remove time accumulation */
         Xp += Ncreep;            /* and add it to read pointer */
         }
      for (i=0; i<IBUFFSIZE-Xp+Xoff; i++)   /* Copy portion of input signal */
         X[i] = X[i+Xp-Xoff];               /* That must be re-used */
      if (last)                             /* If near end of sample... */
         {
         last -= Xp;             /* ...keep track were it ends */
         if (!last)              /* Lengthen input by 1 sample if... */
            last++;              /* ...needed to keep flag TRUE */
	 }
      Xread = i;                 /* Pos in input buff to read new data into */
      Xp = Xoff;

      if (Nout > OBUFFSIZE)      /* Check to see if output buff overflowed */
         fail("Output array overflow");

      writeData((int)Nout ,Y);   /* Write data in output buff to file */
      } while (last >= 0);       /* Continue until done processing samples */
   return(Ycount);               /* Return # of samples in output file */
}




main(argc, argv)
int argc;
char *argv[];
{
   double Factor;               /* Factor = Fout/Fin sample rates */
   double Froll;		/* roll-off frequency */
   double Beta;			/* passband/stopband tuning magic */
   BOOL InterpFilt = TRUE;      /* TRUE means interpolate filter coeffs */
   UHWORD LpScl, Nmult, Nwing;  
   HWORD Imp[MAXNWING];         /* Filter coefficients */
   HWORD ImpD[MAXNWING];        /* ImpD[n] = Imp[n+1]-Imp[n] */
   int outCount;

   Nmult = 37;
   getparams(argc, argv, &Factor, &Froll, &Beta);
   genFilter(Imp, ImpD, &LpScl, Nmult, &Nwing, Froll, Beta);
   resample(Imp, ImpD, LpScl, Nmult, Nwing, Factor, InterpFilt);
   closeData();
}

char *cantmake[5] = {
"0 - no error",
"1 - Nwing too large (Nwing is > MAXNWING)",
"2 - Froll is not in interval [0:1)",
"3 - Beta is < 1.0",
"4 - LpScl will not fit in 16-bits"
};

genFilter(Imp, ImpD, LpScl, Nmult, Nwing, Froll, Beta)
HWORD Imp[MAXNWING];               /* Filter coefficients */
HWORD ImpD[MAXNWING];              /* ImpD[i] = ImpD[i+1] - ImpD[i] */
UHWORD Nmult, *LpScl, *Nwing;
double Froll;
double Beta;
{
   int err;

   *Nwing = Npc*(Nmult+1)/2;     /* # of filter coeffs in right wing */
   *Nwing += Npc/2 + 1;          /* This prevents just missing last coeff */
                                   /*   for integer conversion factors  */
   if ((Froll<=0) || (Froll>1))
         fail("Error: Roll-off freq must be 0<Factor<=1\n");
   if (err = makeFilter(Imp, ImpD, LpScl, *Nwing, Froll, Beta))
         fails("Error: Unable to make filter: %s\n", cantmake[err]);
}