//
//	Analyse a sound file for binaurals
//
//	Hacked down version, no downsampling, limited range
//

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "fidlib.h"

double process_lp(double *coef, double *buf, double val);
double lowpass_step(double *buf, double val);

double coef[7];
typedef struct Chan Chan;
struct Chan {
   double freq;
   double sincos[4];	// Sincos oscillator
   double filt[4][6];	// Four filters: L.re, L.im, R.re, R.im
   double out[2];	// Output magnitudes for left and right
};

#define N_CHAN 140
#define CH_START 138.0
//#define N_CHAN 200
//#define CH_START 75.0
#define CH_STEP 0.1
#define RATE 2004.55
#define BEAT_MIN 0.2	// Minimum frequency difference to test for
#define BEAT_MAX 32.0	// Maximum frequency difference to test for

Chan chan[N_CHAN];

//
//      Setup a sincos generator (i.e. e^ibt for suitable b)
//

inline void 
sincos_init(double *buf, double freq) {
   buf[0]= 1;
   buf[1]= 0;
   buf[2]= cos(freq * 2 * M_PI);
   buf[3]= sin(freq * 2 * M_PI);
}

//
//      Generate the next values from a sincos generator
//

inline void 
sincos_step(double *buf) {
   double v0, v1;
   v0= buf[0] * buf[2] - buf[1] * buf[3];
   v1= buf[0] * buf[3] + buf[1] * buf[2];
   buf[0]= v0;
   buf[1]= v1;
}


//
//	Setup all the channels
//

void 
setup_chan() {
   int a;

   coef[0]= fid_design_coef(coef+1, 6, "LpBe6", 2004.55, 0.1, 0, 0);

   memset(chan, 0, sizeof(chan));
   for (a= 0; a<N_CHAN; a++) {
      double freq= CH_START + CH_STEP * a;
      chan[a].freq= freq;
      sincos_init(chan[a].sincos, freq/RATE);
   }
}

//
//	Process a pair of samples through the entire set of channels
//      

void 
process(double left, double right, int act) {
   int a;
   double re, im;
   for (a= 0; a<N_CHAN; a++) {
      sincos_step(chan[a].sincos);

      re= process_lp(coef, chan[a].filt[0], chan[a].sincos[0] * left);
      im= process_lp(coef, chan[a].filt[1], chan[a].sincos[1] * left);
      if (act) chan[a].out[0]= hypot(re, im);

      re= process_lp(coef, chan[a].filt[2], chan[a].sincos[0] * right);
      im= process_lp(coef, chan[a].filt[3], chan[a].sincos[1] * right);
      if (act) chan[a].out[1]= hypot(re, im);
   }
}

//
//	Ranking table
//

typedef struct Beat Beat;
struct Beat {
   double carr;
   double beat;
   double amp;
};

#define N_TOP 16
Beat beat[N_TOP];

void 
clear_table() {
   memset(beat, 0, sizeof(beat));
}

void 
insert_table(double carr, double freq, double amp) {
   int a;
   for (a= 0; a<N_TOP; a++) 
      if (amp > beat[a].amp) 
	 break;
   
   if (a >= N_TOP) return;
   
   memmove(&beat[a+1], &beat[a], (N_TOP-a-1) * sizeof(Beat));
   
   beat[a].carr= carr;
   beat[a].beat= freq;
   beat[a].amp= amp;
}

#define NAN (0.0/0.0)

//
//	Return the binaural amplitude for a combination
//

inline double 
binamp(int a, int b) {
   double aa= chan[a].out[0];
   double bb= chan[b].out[1];
   return aa < bb ? aa : bb;
}

//
//	Check to see if a particular coordinate is a local maximum.
//	Scan a 7x7 circle (-3->+3 each coord)
//

#define MAXSQ 3
char coord[]= {		// Order checks edges first to eliminate non-max points ASAP
   -3, 0, -2, 2,
   -3, 1, -1, 3,
   -2, 0, -1, 0, 
   -2, 1, -1, 1,
   -1, 2, 0, 0 
};

int 
ismax(int a, int b) {
   double val;
   char *cp;

   if (a < MAXSQ || a >= N_CHAN-MAXSQ ||
       b < MAXSQ || b >= N_CHAN-MAXSQ)
      return 0;

   val= binamp(a,b);

   for (cp= coord; *cp; cp += 2) {
      int ox= cp[0];
      int oy= cp[1];
      if (binamp(a+ox, b+oy) > val ||
	  binamp(a-oy, b+ox) > val ||
	  binamp(a-ox, b-oy) > val ||
	  binamp(a+oy, b-ox) > val)
	 return 0;
   }

   return 1;
}

//
//	Do a big scan over chan[] for binaural combinations
//

void 
binaural_scan() {
   int a, b, diff;
   int min= BEAT_MIN / CH_STEP;
   int max= BEAT_MAX / CH_STEP;
   
   clear_table();

   for (diff= -max; diff <= max; diff++) {
      if (diff > -min && diff < min) continue;
      //printf("  diff %d   \r", diff); fflush(stdout);
      for (a= 0; a<N_CHAN; a++) {
	 b= a + diff;
	 if (!ismax(a,b)) continue;
	 insert_table((chan[a].freq + chan[b].freq) * 0.5, 
		      chan[a].freq - chan[b].freq,
		      binamp(a,b));
      }
   }
}

//
//	Dump out rankings
//

void 
dump_beat() {
   int a;
   for (a= 0; a<N_TOP; a++) {
      if (beat[a].amp >= 0.0001) 
	 printf(" %.1f%+.1f/%.0f", beat[a].carr, beat[a].beat, beat[a].amp * 10000);
   }
}

//
//	Dump out peak frequencies on each side
//

void 
dump_peaks() {
   int a;
   double lmax, lfreq;
   double rmax, rfreq;
   double comm;

   lmax= rmax= 0;
   lfreq= rfreq= 0;

   for (a= 0; a<N_CHAN; a++) {
      if (chan[a].out[0] > lmax) {
	 lfreq= chan[a].freq;
	 lmax= chan[a].out[0];
      }
      if (chan[a].out[1] > rmax) {
	 rfreq= chan[a].freq;
	 rmax= chan[a].out[1];
      }
   }

   // Special hack to pick out other channel for Demo
   if (0) {
      // Assume lmax is bigger; search for an rmax not too close to it

      rmax= 0; rfreq= 0;
      for (a= 0; a<N_CHAN; a++) {
	 if (fabs(chan[a].freq - lfreq) < 3.0) 
	    continue;
	 if (chan[a].out[1] > rmax) {
	    rfreq= chan[a].freq;
	    rmax= chan[a].out[1];
	 }
      }
   }

   comm= lmax < rmax ? lmax : rmax;

   printf(" %.1f%+.1f/%.0f: Left %gHz (%g), Right %gHz (%g)",
	  (lfreq + rfreq) * 0.5, lfreq-rfreq, comm * 10000,
	  lfreq, lmax*10000, rfreq, rmax*10000);
}


//
//	Main routine
//

int 
main(int ac, char **av) {
   int now= 0;
   int step= RATE;			// Analyse data once a second
   int targ= now + step;
   short data[2];
   double dsbuf[2][10];
   double left, right;
   int sec;
   int act;

   setup_chan();
   memset(dsbuf, 0, sizeof(dsbuf));

   while (1) {
      if (2 != fread(data, 2, sizeof(short), stdin)) break;
      left= data[0] * (1.0/32768.0);
      right= data[1] * (1.0/32768.0);

      act= (++now >= targ);
      
      process(left, right, act);

      if (!act) continue;

      targ += step;

      sec= (int)floor(now / RATE);
      printf("%02d:%02d.%02d", sec / 60, sec % 60,
	     ((int)floor((now * 100) / RATE)) % 100);
      dump_peaks();
      printf("\n");

      //      binaural_scan();
      //      
      //      sec= (int)floor(now / RATE);
      //      printf("%02d:%02d.%02d", sec / 60, sec % 60,
      //	     ((int)floor((now * 100) / RATE)) % 100);
      //      dump_beat();
      //      printf("\n");

      fflush(stdout);
   }

   return 0;
}



// Generated by Fiview 0.9.1 <http://uazu.net/fiview/>.  
// All generated example code below is in the public domain.
// Filter 1
// File: -i #1
// Guessed type: low-pass
//
// Frequency-response:
//   Peak gain: 1
//   Guessed 100% gain: 1
//   Regions between 50% (~-3dB) points:
//     0Hz -> 1.37842Hz  (width 1.37842Hz)
//
// Time-response:
//   Sampling rate is 2004.55Hz
//   90% complete after 1246 samples (0.621586s)
//   95% complete after 1354 samples (0.675463s)
//   99% complete after 1708 samples (0.852062s)
//   99.9% complete after 2482 samples (1.23818s)
//   99.99% complete after 3253 samples (1.62281s)
//   Impulse response max deviation is at 832 samples (0.415056s)
//   Impulse response ranges between -2.75644e-05 and 0.00140527
//
// Original filter:
//   x 3.89835803849786e-16
//   / 1 -1.99414726215952 0.994182801911844
//   x 1 2 1
//   / 1 -1.99134673826106 0.991374650355414
//   x 1 2 1
//   / 1 -1.99017163823479 0.990196789212728
//   x 1 2 1 (LpBe6/1)
//
// Filter descriptions:
//   LpBe6/1 == Lowpass Bessel filter, order 6, -3dB frequency 1
//

// Example code for generating any of this class of filters at run-time
double
process_lp(double *coef, double *buf, double val) {
   register double tmp, fir, iir;
   tmp= buf[0]; memmove(buf, buf+1, 5*sizeof(double));
   iir= val * coef[0];
   iir -= coef[1]*tmp; fir= tmp;
   iir -= coef[2]*buf[0]; fir += buf[0]+buf[0];
   fir += iir;
   tmp= buf[1]; buf[1]= iir; val= fir;
   iir= val;
   iir -= coef[3]*tmp; fir= tmp;
   iir -= coef[4]*buf[2]; fir += buf[2]+buf[2];
   fir += iir;
   tmp= buf[3]; buf[3]= iir; val= fir;
   iir= val;
   iir -= coef[5]*tmp; fir= tmp;
   iir -= coef[6]*buf[4]; fir += buf[4]+buf[4];
   fir += iir;
   buf[5]= iir; val= fir;
   return val;
}


// Generated by Fiview 0.9.0 <http://uazu.net/fiview/>.  
// All generated example code below is in the public domain.
// Filter 1
// File: -i#1
// Guessed type: low-pass
//
// Frequency-response:
//   Peak gain: 1
//   Guessed 100% gain: 1
//   Regions between 50% (~-3dB) points:
//     0Hz -> 704.603Hz  (width 704.603Hz)
//
// Time-response:
//   90% complete after 184 samples (0.00417234s @ 44100Hz)
//   95% complete after 226 samples (0.00512472s @ 44100Hz)
//   99% complete after 332 samples (0.00752834s @ 44100Hz)
//   99.9% complete after 491 samples (0.0111338s @ 44100Hz)
//   99.99% complete after 649 samples (0.0147166s @ 44100Hz)
//   Impulse response max deviation is at 77 samples (0.00174603s @ 44100Hz)
//   Impulse response ranges between -0.011811 and 0.0307124
//
// Original filter:
//   x 4.36177017209651e-14
//   / 1 -1.96185441465406 0.970746592664127
//   x 1 2 1
//   / 1 -1.90874985629586 0.917401335922523
//   x 1 2 1
//   / 1 -1.86578842529115 0.87424518063593
//   x 1 2 1
//   / 1 -1.83576856334409 0.844089252549467
//   x 1 2 1
//   / 1 -1.82037037125029 0.828621267578017
//   x 1 2 1 (LpBu10/667)
//
// Filter descriptions:
//   LpBu10/667 == Lowpass Butterworth filter, order 10, -3dB frequency 667
//

// Example code
double
lowpass_step(double *buf, double val) {
   //static double buf[10];
   double tmp, fir, iir;
   tmp= buf[0]; memmove(buf, buf+1, 9*sizeof(double));
   val *= 4.361770172096508e-14;
   iir= val+1.961854414654063*buf[0]-0.970746592664127*tmp;
   fir= iir+buf[0]+buf[0]+tmp;
   tmp= buf[1]; buf[1]= iir; val= fir;
   iir= val+1.908749856295856*buf[2]-0.917401335922523*tmp;
   fir= iir+buf[2]+buf[2]+tmp;
   tmp= buf[3]; buf[3]= iir; val= fir;
   iir= val+1.865788425291151*buf[4]-0.8742451806359305*tmp;
   fir= iir+buf[4]+buf[4]+tmp;
   tmp= buf[5]; buf[5]= iir; val= fir;
   iir= val+1.835768563344089*buf[6]-0.8440892525494667*tmp;
   fir= iir+buf[6]+buf[6]+tmp;
   tmp= buf[7]; buf[7]= iir; val= fir;
   iir= val+1.820370371250291*buf[8]-0.8286212675780171*tmp;
   fir= iir+buf[8]+buf[8]+tmp;
   tmp= buf[9]; buf[9]= iir; val= fir;
   return val;
}



// END //

