/* ptrcq.c     ptch track the output of cQtrans.c 4/91 brown@nadia
   usage: ptrcq -flags  < infile.frm.cqtrans  > outfile
   kernel.number prev calc with genkerm
   infile is the c Q trans of the file
   outfile is a file of midivalues det for this infile
*/

#include <stdio.h>
#include <math.h>
#include <fcntl.h> /* defines O_RDONLY */
#define NMAXFREQS 2048
#define MAXBYTES 1000000
#define NCHNLS 60
#define SRATE  11025.
#define TWOPI  6.2831854 
#define PI  3.1415927 
#define NHARM 6
#define MAXCHNLS 200
#define ATTENFTR .25 /* factor by which atten */

int nharm=NHARM, hspac[41], *hspacptr, nchnls = NCHNLS;
float infloats[MAXBYTES];
float outfloats[MAXBYTES];
float   srate = SRATE;
float  *combptr, comb[MAXCHNLS]; 
double one03, one06, hspacd[21]; 

main(argc,argv)
 int argc;
 char **argv;
{  /* begin main */
        char *cp;
        int flag, debug=0 ;
	int k, comp, n, nr, nk, ncQcmp=NCHNLS, tfrm;
	int  ifp, cQmax =  ncQcmp;
        float   *outfloatp;

	int     print = 0, attenovr = 0; /* atten over 40 for violin */
	int     xcorri=0,xcorrgrph=0;
	int     sumwind, pitch,grphpitch=1,prntpik=0, halfsemi=0;
	int     m, xcorrprnt=0, rempoints, pikpeak=1,p,l,points;
	int     backzero=1, nologftcalc=0, ifp2, fltresp=0, maxreq=0;
	int     hamming = 1, windsiz_1 = 0,siz, calcphase=0, doubcomb=0;
	int     rlcmbg, writehdr=0, dropmidchnls=0, combprint=0;
	int     sumRd = 0, prevpitch = 0, diminCmp=0;
	float   windmax, twopi = TWOPI, pi = PI, tuncorrec=1.00;
	float   neg=0., xcorr[MAXCHNLS], attenftr=ATTENFTR;
	float   negval = -.3;
	float   pitchf;

	one03 = pow(2.,(1./24.)), one06 = pow(2.,(1./12.)); 
        argc--; argv++;                         /* align onto command args */
        while ((cp = *argv) && *cp++ =='-' && (flag = *cp++))
        {
                switch (flag){
                        case 'd': sscanf(cp, "%d", &debug);
                                break;
			case 'c': sscanf(cp, "%d", &combprint);
				break;	break;
			case 'n': sscanf(cp, "%d", &nharm);
				break;
			case 'e': sscanf(cp, "%d", &prntpik);
				break;
			case 'R': sscanf(cp, "%f", &negval);
				doubcomb = 1;   /* enter value of neg comps */
			        break;                  /* with double comb */
			case 'x': sscanf(cp, "%d", &xcorrgrph);
				break;
			case 'g': sscanf(cp, "%d", &grphpitch);
				break;
			case 'a': sscanf(cp, "%d", &attenovr);
				break;
			case 'C': sscanf(cp, "%d", &diminCmp);
				break;
			default: fprintf(stderr, "unknown option\n");
                                break;
                }
                argc--; argv++;
        }
	if(diminCmp)
	  fprintf(stderr, "Attenuating upper harm of comb so highest is .4\n");
	if(attenovr)
	    fprintf(stderr,"ptrcq: Attenuating over chnl %d by %.3f\n",
                        attenovr, attenftr);
        if(diminCmp && doubcomb)
            merde("Dimin comb not implemented for doubcomb.");
              /**** calc harmonic comb spacing **************/
	calccomb(doubcomb,halfsemi,combprint,backzero,negval,neg,diminCmp); 

                     /********* cross correlate   *********/ 
frame:
	if ((nr = read(0, infloats, nchnls*4)) == nchnls*4)
	   sumRd += nr;
	else if (nr < nchnls*4){
	   fprintf(stderr,"Did x corr on %d smples frm stdin\n",sumRd>>2 );
	   exit(0);
	}
	if (attenovr){ /* attenuate violin formant */
	   for (m = attenovr; m < nchnls; ++m)
	     infloats[m] *= attenftr;
	}
       if(doubcomb){ /* do x corr  for doubcomb case */
	  rlcmbg = (int) (log((double)2.)/log(halfsemi?one03:one06));
	  for (k=0; k < nchnls; ++k){	/* begin for of xcorr values */
	     xcorr[k] = 0.; 	/* initialize cross corr for kth point */
	     l = k;
	     m = 0;
	     if (k < rlcmbg){
		combptr = comb + rlcmbg;
		p = nchnls - k;
		m = k;
		while(p--)
		  xcorr[k] += infloats[m++] * (*combptr++);
	     }
	     else {
		combptr = comb;
	        p = nchnls - k + rlcmbg;
		m = k -rlcmbg;
		while(p--){
		   xcorr[k] += infloats[m++] * (*combptr++);
		}
	     }
	  }	/* end "for k etc "calc of nchnls values of xcorr[k] */
       }				/* end "for..." for doubcomb */

       else{ /* for ordinary comb */
	  for (k=0; k < nchnls; ++k){
	     xcorr[k] = 0;          /* initialize cross corr for kth point */

	     m = -1;
	     while(++m < k)               /* mult points before kth by neg*/
	       xcorr[k] += infloats[m] * neg; 

	     p = nchnls - k;		/* p points left from k to nchnls */
	     combptr = comb;		/* start comb on the kth point*/
	     while(p--){
		if(!maxreq || (*combptr && infloats[m-1]<infloats[m] && 
			       infloats[m]>infloats[m+1]))
		{/* if not requiring max or if at a max of logft */
		   xcorr[k] += infloats[m++] * (*combptr++);
                }
		else {  /* max req but no max in fn  or not at the 
				position of a harmonic */
		   xcorr[k] += infloats[m++] * neg; 
		   ++combptr;
	        }
	     } /* end while */
	  } /* end for calc of nchnls values of xcorr[k] */
       }/* end else for ordinary comb */

	if(xcorrprnt){  /* if want print to stderr */ 
	   k = nchnls; l=0;  
	   while(k--)
	     fprintf(stderr,"xcorr[%d]=%.2f\n", l++, xcorr[l]);
	}
   /* end xcorrelation */

        if(xcorrgrph){/* if want to graph the x corr fn */
	   if ((nr = write(1, xcorr,nchnls*4 )) <= 0)
	     merde("Problem writing output file");
	}
	if(pikpeak){
	  prevpitch = pitch;
	  pitch = 53 + peakpicker(xcorr,nchnls);
	  if(prntpik){ /* to print out pitch picked to stderr */
	    if (pitch != prevpitch)
	      fprintf(stderr,"\n", pitch); 
	    fprintf(stderr,"%d ", pitch);
	 }
	  if(grphpitch){  /* to graph pitch picked */
	     pitchf = (float)pitch;
	     write(1, &pitchf, 4);
	  }
       }
goto frame;
     } /* end main */

merde(s)
 char *s;
{
        fprintf(stderr,"MERDE, ALORS!   %s\n",s); 
        exit(1);
}

peakpicker(inarr, arrlen)
float inarr[];
int arrlen;
{
   float maxval = 0., *inarrp = inarr;
   int chnatmax = 0, l; /* channel where max occurs */

   for (l = 0 ; l < arrlen; ++l){
      if( *inarrp > maxval){
	 maxval = *inarrp ;
	 chnatmax = l;
      }
      ++inarrp;
   }
   return chnatmax;
}
      /******* calculate comb spacing ****************/
calccomb(dbcb,hlfsmi,cmbprnt,bckzro,ngvl,ng,dmncmp)
      /*	calccomb(doubcomb,halfsemi,combprint,backzero,negval,neg); */
int dbcb,hlfsmi,cmbprnt,bckzro;
float ngvl,ng;

{ /* begin comb calc */
        int l,m,k,p;
	float amt;

	if(dbcb){
	   fprintf(stderr,
	    "Using double the harm to suppress high octave error. ");
	   nharm *= 2;              /* double the no of comps as use evens */
	   ngvl = ((ngvl > 0.)? -ngvl : ngvl);    /*can enter as pos or neg*/
	   fprintf(stderr, "Negval = %.3f\n",ngvl);
	}

	p = nharm;  l=1; 
	while(p--){ /*calc position of harmonics hspac[l] in channels
		      for nharm harmonic components */
	   if(hlfsmi==1)hspacd[l] = log((double)l)/log(one03);/*qrter ton spc*/
	   else hspacd[l] = log((double)l)/log(one06);     /*semitone spacing*/
	   hspac[l] = (int)(hspacd[l] + .5);
	   if(cmbprnt)
	     fprintf(stderr,
	       "nharm %d hspacd[%d]= %.2f\tdiff=%.2f (int)hspac= %d\n"
		,l,l,hspacd[l],hspacd[l]-hspacd[l-1], hspac[l]);
	   ++l;
	}
	hspacptr = hspac; ++hspacptr;            /* harmonic spacing pointer */
	k = nchnls; l = 0;
	if(bckzro) ng = 0.;               /* don't make comb average to zero */
	else
	  ng = -(float)nharm/(nchnls - nharm);         /* make ave of comb 0 */
	while (k--){
	   if(dbcb){                                       /* if double comb */
	      m = hspacptr - hspac;        /*m keeps track of even or odd cmp*/
	      comb[l] = (l == *hspacptr ? (m % 2 == 0 ? 1. : ngvl) : ng);
	   }     /* even comps = 1 and odd comps = the negval that was input */
	   else comb[l] = (l == *hspacptr ? 1. : ng); /* comb = 1 at
			the position of a harmonic position and neg elsewhere*/
	          /* NB: negval has to be initialized or step below messes up*/
	   if ((comb[l]==1. || comb[l]==ngvl) && l < hspac[nharm]) 
	     ++hspacptr;       /* if at pos of harm, step up harm spacing ptr*/
	   if (cmbprnt)
	     fprintf(stderr, "comb[%d] = %.1f\n", l, comb[l]);
	   ++l;                                 /* next member of comb array */
	}
	if(dmncmp) {/* dec amplitude for higher comb comps */
	  amt = .6/(nharm-1);                       /* calc amt to dec by */
                                      /* subtract from pos nonzero comps*/
	                              /* add to neg nonzero comps */
	  l = -1; hspacptr = hspac + 1;
	  while(++l < nharm){ /* amount * the harmonic number - 1 */
	    comb[ *hspacptr++ ] -= amt * l ;
	    if (cmbprnt)
	      fprintf(stderr, "l=%d comb[%d] = %.1f\n",l, *(hspacptr-1),
		      comb[ *(hspacptr-1)]);
	  }
       }
     }/*end comb calc*/