/*
 *  tcma:  Tesla Coil Mode Analyser
 *
 *      origin: http://www.abelian.demon.co.uk/tssp/tcma/
 *  maintainer: Paul Nicholson, paul@abelian.demon.co.uk
 *
 *  Many thanks to Terry Fritz, <twftesla@uswest.net>, for invaluable help
 *  in providing test data.
 *
 *  See the above website for documentation and the latest version of
 *  this code.
 *
 *  This code is written to compile without complaint using gcc under Linux.
 *  Porting: you're on your own.
 */

#define VERSION "0.1g, 24th April 2002"

/*
 *  Tunables.
 */

#define MAXPEAKS 3000 /* 
                       *  Max number of peaks to consider in initial assessment
                       *  of the input waveform.
                       */

#define FILTER_BW 5     /*
                         * Percentage bandwidth (+/-) of the component filter.
                         */ 

//#define MAXDATA 16384              /* Max number of records in an input file */
#define MAXDATA 32768              /* Max number of records in an input file */

#define DEFAULT_NPEAKS 4  /*
                           *  By default, examine this many of the strongest
                           *  frequency components.
                           */

#define MINFREQ 30000   /*
                         *  Ignore any signals below this frequency.
                         */

#define THRESHOLD_START 0.1
#define THRESHOLD_END 0.1

/*
 *  System related stuff.
 */

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/*
 *  Flags set by the command line options.
 */

int FLAG_FT = 0;           /* Set if a fourier transform output is requested */
int FLAG_TD0 = 0;
int FLAG_TD1 = 0;
int FLAG_TD2 = 0;
int FLAG_QCMP = 0;        /* Request output of Q-factor correlation function */
int FLAG_FCMP = 0;       /* Request output of frequency correlation function */
int FLAG_PCMP = 0;           /* Request output of phase correlation function */
int FLAG_PRE = 0;         /* Make the above function outputs pre-convergence */
int FLAG_V = 0;                                    /* Request verbose output */
int FLAG_SIGNALS = 0;      /* The program is to just report the approx peaks */
int FLAG_DUAL = 0;                      /* Optimise for closely spaced peaks */
int FLAG_PMAX = DEFAULT_NPEAKS;                /* Number of peaks to process */
int FLAG_Q = 0;                                               /* Run quietly */
int FLAG_NOHARM = 0;
int FLAG_U8 = 0;                          /* Input data is in unsigned bytes */
int FLAG_S8 = 0;                            /* Input data is in signed bytes */
double FLAG_TSTEP = 0;

double FLAG_start_us = -1;
double FLAG_end_us = -1;
int trace_start;               /* Index of actual trace start in work arrays */
int trace_end;                                         /* Index of trace end */

double t1, t2;                /* First and last timestamps of the input file */
double DT;                              /* Time interval per input file step */
double DF;                                    /* Frequency interval per step */
double F_LB, F_UB;                 /* Bandpass filter lower and upper limits */
int start_n;        /* Start index of the actually used portion of the input */
int end_n;                                  /* End index of the used portion */
int from_peak, to_peak, do_peak = -1;

double fmin = MINFREQ, fmax = 0;

typedef struct {
   double re;
   double im; 
} COMPLEX;

COMPLEX inputdata[MAXDATA+1];      /* TD input signal, relocated and cropped */

double raw_data[ MAXDATA];           /* Holds the input file data as read in */

void bailout( char *format, ...)
{
   char temp[ 500];

   vsprintf( temp, format, (char **)(&format + 1));
   fprintf( stderr, "bailout: %s\n", temp);
   exit( 1);
}

void *safe_malloc( int n)
{
   void * p = malloc( n);
   if( !p) bailout( "out of memory");
   return p;
}

static double rms_magnitude( COMPLEX *T)
{
   int i;
   double mag = 0;

   for( i=1; i<=MAXDATA; i++) mag += T[i].re * T[i].re + T[i].im * T[i].im;
   return sqrt( mag)/MAXDATA;
}

static void ca_add( COMPLEX *D, COMPLEX *S)
{
   int i = MAXDATA; while( i--) (++D)->re += (++S)->re;
}

static void ca_sub( COMPLEX *D, COMPLEX *S)
{
   int i = MAXDATA; while( i--) (++D)->re -= (++S)->re;
}

static COMPLEX * ca_alloc( void)
{
   return safe_malloc( sizeof( COMPLEX) * (MAXDATA+1));
}

static void ca_mul( COMPLEX *D, double a)
{
   int i = MAXDATA; while( i--) (++D)->re *= a;
}

static void ca_zero( COMPLEX *D)
{
   int i = MAXDATA; while( i--) *++D = (COMPLEX) {0,0};
}

/*
 *  A structure for each frequency component.
 */

struct PEAK {
  double F;                                                /* Frequency - Hz */
  double G;                                              /* Amplitude factor */
  double Q;                                                      /* Q factor */
  double P;              /* Phase angle - radians, wrt zero at index start_n */
  double v;                              /*  Amplitude of containing FT step */

  double error_Q;            /* Estimated error of Q determination - percent */
  double error_F;            /* Estimated error of F determination - percent */

  int rcnt;
  double Qx;
  
  char *error;
 }
  peaks[MAXPEAKS];

static int npeaks = 0;                       /* Number of peaks in the array */

static void shuffle_peaks( int n)
{
   if( !npeaks) bailout( "error in shuffle_peaks");

   while( n < npeaks-1)
   {
      peaks[n] = peaks[n+1];
      n++;
   }

   npeaks--;
}

static double sign( double t)
{
   if( t < 0) return -1;
   if( t > 0) return 1;
   return 0;
}

/*
 *  Forward and reverse fft.
 */

static void nfft( COMPLEX *v, int isign)
{
   unsigned n, mmax, m, j, i, istep;
   double wtemp, wr, wpr, wpi, wi, theta;
   double tempr, tempi;
   double *fdata = 1 + (double *) v;

   n = MAXDATA << 1;
   j = 1;
   for( i=1; i<n; i+= 2)
   {
      if( j > i)
      {
         double t = fdata[j]; fdata[j] = fdata[i];  fdata[i] = t;
         t = fdata[j+1]; fdata[j+1] = fdata[i+1];  fdata[i+1] = t;
      }
      m = n >> 1;
      while( m >= 2 && j > m)
      {
         j -= m;
         m >>= 1;
      }
      j += m;
   }

   mmax  = 2;
   while( n > mmax) 
   {
      istep = mmax << 1;
      theta = isign * 2*M_PI/mmax;
      wtemp = sin( 0.5 * theta);
      wpr = -2 * wtemp * wtemp;
      wpi = sin(theta);
      wr = 1;
      wi = 0;
      for( m = 1; m < mmax; m += 2)
      {
         for( i = m; i <= n; i += istep) 
         {
            int j = i + mmax;
            double *fj = fdata + j;
            double *fi = fdata + i;

            tempr = wr * *fj - wi * *(fj+1);
            tempi = wr * *(fj+1) + wi * *fj;
            *fj++ = *fi - tempr;
            *fi++ += tempr;
            *fj = *fi - tempi;
            *fi += tempi;
         }
         wr = (wtemp=wr) * wpr - wi*wpi+wr;
         wi = wi*wpr + wtemp*wpi+wi;   
      }
      mmax = istep;
   }

   if( isign < 0) for( i=1; i<= MAXDATA << 1; i++) fdata[i] /= MAXDATA;
}

#define forward_fft( v) nfft( (v), 1)
#define reverse_fft( v) nfft( (v), -1)

/*
 *  Apply a bandpass filter to the fourier components in data.  Simply zero
 *  anything outside the passband.
 */

void crop_ft( COMPLEX *data, double low_lim, double high_lim)
{
   int i, lb = (int)(low_lim/DF), lh = (int)(high_lim/DF);

   for( i=1; i<=lb; i++)
   {
      data[i].re = 0;
      data[i].im = 0;
      data[MAXDATA-i+1].re = 0;
      data[MAXDATA-i+1].im = 0;
   }
   for( i=lh; i<=MAXDATA/2; i++)
   {
      data[i].re = 0;
      data[i].im = 0;
      data[MAXDATA-i+1].re = 0;
      data[MAXDATA-i+1].im = 0;
   }
}

/*
 *  Initial assessment of the waveform in the time domain.  The useable part
 *  of the trace is determined.
 */

static void crop_td( void)
{
   int n_peak = 1;
   int i;
   double peak_mag = 0;

   /*
    *  Locate the peak of the entire input signal.
    */

   for( i=1; i<=MAXDATA; i++) 
   {
      double x = inputdata[i].re;

      if( x < 0) x = -x;

      if( x > peak_mag) 
      {
         peak_mag = x;
         n_peak = i;
      }
   }

   if( FLAG_V) 
      fprintf( stderr, "impulse peaks at around %.3f mS (step %d)\n",
                         1000 * (t1 + (n_peak-trace_start+0.5)*DT), n_peak);

   if( FLAG_end_us >= 0)
   {
      end_n = (FLAG_end_us*1e-6 - t1)/DT + trace_start;
   }
   else
   {
      /*
       *  Work backwards from the end to find the point where the signal last
       *  exceeded a factor THRESHOLD_END of the peak value.
       */
   
      for( i=MAXDATA; i>n_peak; i--)
      {
         double x = inputdata[i].re;
   
         if( x < 0) x = -x;
         if( x > peak_mag * THRESHOLD_END) break;
      }
   
      end_n = i;   /* The last index of the used part of the trace */
   }

   if( FLAG_start_us >= 0)
   {
      start_n = (FLAG_start_us*1e-6 - t1)/DT + trace_start;
   }
   else
   {
      /*
       *  Work forwards from the trace start to find the point where the signal
       *  first exceeds a factor THRESHOLD_START of the peak value.
       */
   
      for( i=1; i<=n_peak; i++)
      {
         double x = inputdata[i].re;
   
         if( x < 0) x = -x;
         if( x > peak_mag * THRESHOLD_START) break;
      }
   
      start_n = i;  /* The first index of the used part of the trace */
   }

   /*
    *  Zero the unwanted parts of the input data.
    */

   for( i=1; i<start_n; i++) inputdata[i].re = 0;
   for( i=MAXDATA; i>end_n; i--) inputdata[i].re = 0;

   if( FLAG_V)
   { 
      fprintf( stderr, "start=%d end=%d\n", start_n, end_n);
      fprintf( stderr, "Analysing from %.3f mS to",
                         1000 * (t1 + DT * (start_n-trace_start+1)));
 
      fprintf( stderr, " %.4f mS\n", 
                         1000 * (t1 + DT * (end_n-trace_start+2)));
   }
}

/*
 *  Copy a complex array.
 */

static void ca_copy( COMPLEX *D, COMPLEX *S)
{
   memcpy( D, S, sizeof( COMPLEX) * (MAXDATA+1));
}

/*
 *  Multiply T by a preconditioning envelope.
 */

static void precondition_peak( COMPLEX *T, struct PEAK *p)
{
   int end = start_n + 4 * log10(THRESHOLD_END)/log10(1-2*M_PI/p->Qx)/p->F/DT;
   int i;

   if( end > end_n) end = end_n;

end = end_n;
// printf( "start %d special end %d\n", start_n, end); 
   T += start_n-1;
   for( i=start_n; i<=end; i++)
   {
      double t = (i - start_n + 1)/(double) (end - start_n + 1) * M_PI;
      (++T)->re *= sin(t);
   }
}

static void precondition( COMPLEX *T)
{
   int i;
 
   T += start_n-1;
   for( i=start_n; i<=end_n; i++)
   {
      double t = (i - start_n + 1)/(double) (end_n - start_n + 1) * M_PI;
      (++T)->re *= sin(t);
   }
}

/*
 *  Synthesize a waveform for the given peak.
 */

static void synthesize_x( COMPLEX *T, int nofilter, struct PEAK *p)
{
   int i;
//   double g = sqrt(1-1.0/4/p->Q/p->Q);
   double g = 1.0;
   double r = -M_PI * p->F/p->Q * DT;
   double f = g * 2 * M_PI * p->F * DT;

   for( i=1; i< start_n; i++) *T++ = (COMPLEX) {0,0};
   
   for( ; i<=end_n; i++)
   {
      double a = sin( p->P + f * (i-start_n));
      double g = exp( r * (i-start_n));
      
      *T++ = (COMPLEX) {a * g, 0};
   }

   for( ; i<=MAXDATA; i++) *T++ = (COMPLEX) {0,0};
}

static void synthesize( COMPLEX *T, int nofilter, struct PEAK *p)
{
   synthesize_x( T, nofilter, p);
  
   precondition_peak( T, p);
   // precondition( T);
 
   if( !nofilter)
   {
      forward_fft( T);
      crop_ft( T, F_LB, F_UB);
      reverse_fft( T);
   }
}

static void synthesize_noprecond( COMPLEX *T, int nofilter, struct PEAK *p)
{
   synthesize_x( T, nofilter, p);
  
   if( !nofilter)
   {
      forward_fft( T);
      crop_ft( T, F_LB, F_UB);
      reverse_fft( T);
   }
}

#if 0
static void remove_noise( COMPLEX *D)
{
   int i, n;

   for( n=0; n<1; n++)
   for( i=2; i<=MAXDATA/2-1; i++)
   {
      COMPLEX *t = D + i - 1;
      COMPLEX A;
      double a;

      A = *t;
      t += 2;
      A.re += t->re;
      A.im += t->im;
      A.re /= 2;
      A.im /= 2;
      a = sqrt( A.re*A.re + A.im*A.im);
      t--;
      if( sqrt( t->re*t->re + t->im*t->im) > 5 * a/2) {
        D[i] = (COMPLEX){0,0};;
        D[MAXDATA-i+1] = (COMPLEX){0,0};
      }
   }
}
#endif

static void eq( COMPLEX *T)
{
   return;
#if 0
   int n;
   double *eq = safe_malloc( MAXDATA * sizeof( double));
   #define BA (MAXDATA/30)

   for( n=1; n<=MAXDATA/2; n++)
   {
      int k;
      int u1 = n - BA;
      int u2 = n + BA;

      if( u1 < 1) { u1 = 1; u2 = u1 + 2*BA; }
      if( u2 > MAXDATA/2) { u2 = MAXDATA/2; u1 = u2 - 2*BA; }
      eq[n] = 0;
      for( k=u1; k<=u2; k++) eq[n] += T[k].re;
      eq[n] /= 2*BA;
   }
  
   for( n=1; n<=MAXDATA/2; n++) T[n].re /= eq[n];

   free( eq);
#endif
}

static void output_FT( void)
{
   int w;
   COMPLEX *T = ca_alloc();

   ca_copy( T, inputdata);
   precondition( T);
   forward_fft( T);
//    remove_noise( T);

   for( w=1; w<=MAXDATA/2; w++) 
   {
      double r = T[w].re; 
      double i = T[w].im;
      T[w].re = sqrt( r*r + i*i);
   }

   eq( T);
   for( w=1; w<=MAXDATA/2; w++) printf( "%.5e %.5e\n", w*DF, T[w].re);

   free( T);
}

static double compare_G( COMPLEX *T, COMPLEX *S)
{
   int i;
   double st = 0, ss = 0;

   for( i=1; i<=MAXDATA; i++)
   {
      st += T[i].re * T[i].re;
      ss += S[i].re * S[i].re;
   }

   return sqrt( st/ss);

}

/*
 *  A standard correlation function.
 */

double corr_X( COMPLEX *VA, COMPLEX *VB)
{
   int i = MAXDATA;
   double s = 0, ha = 0, hb = 0;

   while( i--)
   {
      double a = (VA++)->re;
      double b = (VB++)->re;

      s += a * b;  
      ha += a * a;
      hb += b * b;
   }

   return s/sqrt(ha * hb);
}

/*
 *  A correlation function which works on the smoothed mean amplitude of the
 *  waveforms, thus discarding phase and frequency differences.
 */

double corr_M( COMPLEX *VA, COMPLEX *VB)
{
   int i = MAXDATA;
   double smooth = 0.99;
   double s = 0, ha = 0, hb = 0;
   double ma = 0, mb = 0;

   while( i--)
   {
      double a = (VA++)->re;
      double b = (VB++)->re;

      if( a < 0) a = -a;
      if( b < 0) b = -b;

      ma = ma * smooth + a * (1-smooth);
      mb = mb * smooth + b * (1-smooth);

      s += ma * mb;  
      ha += ma * ma;
      hb += mb * mb;
   }

#if 0
   if( ha == 0 || hb == 0) 
   {
      fprintf( stderr, "zero array in corr_M\n");
      return 0;
   }
#endif

   if( ha == 0 || hb == 0) bailout( "zero array in corr_M");

   return s/sqrt(ha * hb);
}

#if 0
double corr_MF( COMPLEX *VA, COMPLEX *VB, double F)
{
   int i;
   int steps = (int)(0.5 + 1/F/DT);
   double smooth = 1 - 1.0/(steps);
   double s = 0, ha = 0, hb = 0;
   double ma = 0, mb = 0;

   if( steps <= 2) bailout( "too few steps in corr_MF");
 
   for( i=1; i<=MAXDATA; i++) 
   {
      double a = VA[i].re;
      double b = VB[i].re;

      if( a < 0) a = -a;
      if( b < 0) b = -b;

      ma = ma * smooth + a * (1-smooth);
      mb = mb * smooth + b * (1-smooth);

      s += ma * mb;  
      ha += ma * ma;
      hb += mb * mb;
   }

   return s/sqrt(ha * hb);
}
#endif

/*
 *  A correlation which discards amplitude information and responds only to the
 *  phase differences.
 */

double corr_P( COMPLEX *VA, COMPLEX *VB)
{
   int i = MAXDATA, n = 0;
   double s = 0;

   while( i--)
   {
      double a = (VA++)->re;
      double b = (VB++)->re;
      int c = sign( a * b);  
      if( c) { s += c; n++; }
   }

   return s/n;
}

static void output_Q_function( COMPLEX *T, struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   struct PEAK t = *p;

   for( t.Q = 1; t.Q < 2000; t.Q += 10)
   {
      synthesize( S, 0, &t);
      printf( "%.1f %.4e\n", t.Q, corr_M( S, T));
   }

   free( S);
}

/*
 *  A correlation function which compares magnitudes in the frequency domain,
 *  this discarding phase differences.
 */

static double corr_F( COMPLEX *TA, COMPLEX *TB)
{
   int i;
   double s = 0, ha = 0, hb = 0;

   for( i=1; i<=MAXDATA/2; i++) 
   {
      double a = sqrt( TA[i].re * TA[i].re + 
                       TA[i].im * TA[i].im);
      double b = sqrt( TB[i].re * TB[i].re + 
                       TB[i].im * TB[i].im);

      s += a * b;  
      ha += a * a;
      hb += b * b;
   }

   return s/sqrt(ha * hb);
}

static void output_F_function( struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   COMPLEX *T = ca_alloc();
   struct PEAK t = *p;
   double b = F_UB - F_LB;

   ca_copy( T, inputdata); 
   precondition( T);
   forward_fft( T);
   crop_ft( T, F_LB, F_UB);

   for( t.F = p->F - b/2; t.F < p->F + b/2; t.F += b/100)
   {
      synthesize( S, 1, &t);
      forward_fft( S);
      crop_ft( S, F_LB, F_UB);

      printf( "%.2f %.5e\n", t.F, corr_F( S, T));
   }

   free( T); free( S);
}

static void output_P_function( COMPLEX *T, struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   struct PEAK t = *p;

   for( t.P = - M_PI/1; t.P < M_PI/1; t.P += M_PI/128)
   {
      synthesize( S, 0, &t);
      printf( "%.5f %.5e\n", t.P/M_PI, corr_P( S, T));
   }

   free( S);
}

static void output_TD1( COMPLEX *TA, struct PEAK *p)
{
   int w;
   double g;
   COMPLEX *T = ca_alloc();

   synthesize( T, 0, p);
   g = compare_G( TA, T);

   for( w=1; w<=MAXDATA; w++) 
      printf( "%d %.5e %.5e\n", w, TA[w].re, g * T[w].re);

   free( T);
}

static void output_TD0( COMPLEX *TA, struct PEAK *p)
{
   int w;
   double g;
   COMPLEX *T = ca_alloc();

   synthesize_noprecond( T, 1, p);
   g = compare_G( inputdata, T);

   for( w=1; w<=MAXDATA; w++) 
      printf( "%d %.5e %.5e\n", w, TA[w].re, g * T[w].re);

   free( T);
}

/*
 *  Read in the input file.
 */

static void read_raw_data( void)
{
   int i;
   int NR = 0;
   char temp[ 100];

   if( FLAG_TSTEP == 0)
   {
      while( fgets( temp, 100, stdin))
      {
         char *p;
         p = strchr( temp, '\r');  if( p) *p = 0;
         p = strchr( temp, '\n');  if( p) *p = 0;
         p = strchr( temp, ' ');
         if( !p) p = strchr( temp, ',');
         if( !p || !*p) continue;
         if( !p) bailout( "invalid input format [%s]", temp);
         *p = 0;
         t2 = atof( temp);  
         if( !NR) t1 = t2;
         if( ++NR > MAXDATA) bailout( "too many input records");
         raw_data[NR]  = atof( p+1);
      }
   
      DT = (t2-t1)/(NR-1);
   }
   else
   {
      while( fgets( temp, 100, stdin))
      {
         if( ++NR > MAXDATA) bailout( "too many input records");
         raw_data[NR]  = atof( temp);
      }
   
      DT = FLAG_TSTEP;
      t1 = 0;
      t2 = DT * (NR-1);
   }

   DF = 1/DT/MAXDATA;

   if( FLAG_V)
   {
      fprintf( stderr, "trace samples %d, step %.3e seconds, ", NR, DT);
      fprintf( stderr, "total duration %.4f mS\n", MAXDATA * DT * 1000);
      fprintf( stderr, "FT resolution %.1f Hz\n", DF);
   }

   /*
    *  Position the raw input data centrally in the inputdata array.
    */

   trace_start = 1 + (MAXDATA - NR)/2;
   trace_end = trace_start + NR - 1;

   if( FLAG_V) fprintf( stderr, "trace start at %d\n", trace_start);

   for( i=1; i < MAXDATA; i++)
   {
      inputdata[ i].re  = 0;
      inputdata[ i].im  = 0;
   }

   for( i=1; i<=NR; i++)
   {
      inputdata[trace_start + i - 1].re  = raw_data[i];
      inputdata[trace_start + i - 1].im  = 0;
   }
}

/*
 *  Amplitude comparision function used by qsort on the peaks array.
 */

static int peak_cmp( const void *p1, const void *p2)
{
   if( ((struct PEAK *)p1)->v > ((struct PEAK *)p2)->v) return -1;
   if( ((struct PEAK *)p1)->v < ((struct PEAK *)p2)->v) return 1;
   return 0;
}

static void scan_peaks( void)
{
   COMPLEX *T = ca_alloc();
   int w = 3;  
   int n;
   double v1 = 0, v2 = 0, v3 = 0;

   ca_copy( T, inputdata);
   precondition( T);
   forward_fft( T);

   for( n=1; n<=MAXDATA/2; n++)
   {
      COMPLEX *t = T + n;
      t->re = sqrt( t->im*t->im + t->re*t->re);
   }

   eq( T);

   for( n=w; n+w<=MAXDATA/2; n++) 
   {
      int i;
      double val = 0;
      for( i = -w; i <= w; i++)
      {
         COMPLEX *t = T + n + i;
         val += t->re;
      }
      
      v1 = v2;
      v2 = v3;
      v3 = val;

      if( v2 > v1 && v2 > v3)
      {
         struct PEAK *p = peaks + npeaks;
         p->F = (n-2) * DF; 
         p->v = v2;
         p->G = 0;
         p->rcnt = 0;
         p->error = 0;
         p->P = 0; 
         p->Q = (end_n - start_n) * DT * p->F;
         p->Qx = p->Q;

         if( peaks[npeaks].F > fmin)
            if( !fmax || peaks[npeaks].F <= fmax) npeaks++;
      }

      if( npeaks == MAXPEAKS) break;
   }

   free( T);

   if( FLAG_V) fprintf( stderr, "found %d peaks\n", npeaks);

   qsort( peaks, npeaks, sizeof( struct PEAK), peak_cmp);
   for( n=1; n<npeaks; n++) if( peaks[n].v < peaks[0].v/1000) npeaks = n;
   if( npeaks > FLAG_PMAX) npeaks = FLAG_PMAX;

   free( eq);
}

static double find_F( COMPLEX *W, struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   COMPLEX *T = ca_alloc();
   #define A 20
   struct PEAK t = *p;
   double b = F_UB - F_LB;
   int n = 0, maxn = -1;
   double m[A];
   double maxm = -1;
   double Cl=0, Cm=0, Ch=0;
   double Fl, Fm, Fh;

    ca_copy( T, W);
    reverse_fft( T);

   for( n=0; n<A; n++) 
   {
      t.F = p->F - b/2 + n * b/A;

      synthesize( S, 1, &t);
      forward_fft( S);
      crop_ft( S, F_LB, F_UB);

      m[n] = corr_F( S, T);
   }
  
   for( n=0; n<A; n++) if( m[n] > maxm) { maxm = m[n]; maxn = n; }

   if( maxn < 1 || maxn >= A-1)
   {

      fprintf( stderr, "unable to estimate F\n");
      return p->F;
      // bailout( "unable to estimate F");
   }

   Fl = p->F - b/2 + (maxn-1) * b/A;
   Fm = p->F - b/2 + (maxn) * b/A;
   Fh = p->F - b/2 + (maxn+1) * b/A;

   n = 0;

   while( 2*(Fh - Fl)/(Fh+Fl) > 0.0001)
   {
      double c;
      if( Fh - Fm > Fm - Fl) t.F = (Fh + Fm)/2;
      else t.F = (Fm + Fl)/2;

      synthesize( S, 1, &t);
      forward_fft( S);
      crop_ft( S, F_LB, F_UB);

      c = corr_F( S, T);

      if( c > Cm)
      {
         Cm = c; Fm = t.F;
      }
      else if( t.F < Fm) { Cl = c; Fl = t.F; }
      else { Ch = c; Fh = t.F; }

      if( n++ > 100)
         bailout( "cannot converge in find_F (2)");
   }

   free( T); free( S);
   return Fm;
}

static double find_P( COMPLEX *Tdata, struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   struct PEAK t = *p;
   #define W 32
   int k, n = 0, maxk = -1;
   double maxm = -1;
   double Cl=0, Cm=0, Ch=0;
   double Pl, Pm, Ph;
   double m[W];

   for( k=0; k<W; k++)
   {
      t.P = -M_PI + (2*M_PI * k)/W;  
      synthesize( S, 0, &t);
       
      m[k] = corr_P( S, Tdata);
   }

   for( k=0; k<W; k++) if( m[k] > maxm) { maxm = m[k]; maxk = k; }

   if( maxk < 0) bailout( "unable to estimate phase");

   Pm = -M_PI + (2*M_PI*maxk)/W;
   Pl = Pm - 2*M_PI/W;
   Ph = Pm + 2*M_PI/W;

   t.P = Pl; synthesize( S, 0, &t); Cl = corr_P( S, Tdata);
   t.P = Ph; synthesize( S, 0, &t); Ch = corr_P( S, Tdata);
   
   while( 1)
   {
      Pm = (Pl + Ph)/2;
      t.P = Pm; synthesize( S, 0, &t); Cm = corr_P( S, Tdata);
  
      if( Cm > Cl && Cm > Ch) break;
      if( Cm > Cl) { Cl = Cm; Pl = Pm; }
      else 
      if( Cm > Ch) { Ch = Cm; Ph = Pm; }
      else 
      {
         free( S);
         return Pm;
      }
      if( n++ > 100) bailout( "cannot converge in find_P (1)");
   }

   n = 0;

   while( 2*(Ph - Pl)/M_PI > 0.0001)
   {
      double c;
      if( Ph - Pm > Pm - Pl) t.P = (Ph + Pm)/2;
      else t.P = (Pm + Pl)/2;

      synthesize( S, 0, &t); c = corr_P( S, Tdata);

      if( n++ > 100)
      {
         fprintf( stderr, "Pl=%.6e Pm=%.6e Ph=%.6e p=%.6e\n",
                           Pl/M_PI, Pm/M_PI, Ph/M_PI, t.P/M_PI);
         fprintf( stderr, "Cl=%.6e Cm=%.6e Ch=%.6e c=%.6e\n", Cl, Cm, Ch, c);
         bailout( "cannot converge in find_P (2)");
      }
      if( c > Cm)
      {
         Cm = c; Pm = t.P;
      }
      else if( t.P < Pm) { Cl = c; Pl = t.P; }
      else { Ch = c; Ph = t.P; }
   }

   free( S);
   return Pm;
}

static double find_Q( COMPLEX *T, struct PEAK *p)
{
   COMPLEX *S = ca_alloc();
   struct PEAK t = *p;
   double Cl=0, Cm=0, Ch=0;
   double Ql = 1;
   double Qh = 2000;
   double Qm;
   int n = 0;

   t.Q = Ql; synthesize( S, 0, &t); Cl = corr_M( S, T);
   t.Q = Qh; synthesize( S, 0, &t); Ch = corr_M( S, T);

   while( 1)
   {
      Qm = sqrt( Ql * Qh);
      t.Q = Qm; synthesize( S, 0, &t);
      Cm = corr_M( S, T);
      if( Cm > Cl && Cm > Ch) break;
      if( Cm > Cl) { Cl = Cm; Ql = Qm; }
      else { Ch = Cm; Qh = Qm; }
      n++;
   }

   n = 0;

   while( (Qh - Ql)/sqrt( Qh * Ql) > 0.0001)
   {
      double c;
      if( Qh - Qm > Qm - Ql) t.Q = sqrt( Qh * Qm);
      else t.Q = sqrt( Qm * Ql);

      synthesize( S, 0, &t); c = corr_M( S, T);

      if( c > Cm) { Cm = c; Qm = t.Q; }
      else if( t.Q < Qm) { Cl = c; Ql = t.Q; }
      else { Ch = c; Qh = t.Q; }
   }

   free( S);
   return Qm;
}

static void heading_progress( void)
{
   if( !FLAG_Q)
   {
      fprintf( stderr, "      Q      F      P   ");
      fprintf( stderr, "   corrX  corrP    corrM err\n");
   }
}
 
static double report_progress( COMPLEX *T, COMPLEX *S, 
                               int e, char c, struct PEAK *p)
{
   double Cx = corr_X( S, T),
          Cp = corr_P( S, T),
          Cm = corr_M( S, T);

   double error = (1 - (corr_X( S, T) *
                        corr_P( S, T))) * 100;

   error = ((int)(error * 10 + 0.5))/10.0;

   if( !FLAG_Q)
   {
      fprintf( stderr, 
        "%c %.3f %.0f %+.3f %+.6f %+.3f %.6f", 
           c,
           p->Q, p->F, p->P/M_PI, 
           Cx, Cp, Cm);
   
      if( e) fprintf( stderr, " %.1f%%", error);
   
      fprintf( stderr, "        ");
      if( FLAG_V) fprintf( stderr, "\n");
      else fprintf( stderr, "\r");
   }

   return error;
}

static double estimate_F_error( COMPLEX *T, struct PEAK *p)
{
   struct PEAK u = *p;
   COMPLEX *S = ca_alloc();
   COMPLEX *test = ca_alloc();
   double target_cor, r1, r2, error = 0.01/100;
   double factor = 10;
   double max_error;

   ca_copy( test, T);
   reverse_fft( test);

   synthesize( S, 1, &u);
   forward_fft( S);
   crop_ft( S, F_LB, F_UB);
   target_cor = corr_F( S, test);

   while( factor > 1.2)
   {
      u.F = p->F * (1+error); synthesize( S, 0, &u);
      forward_fft( S);
      r1 = corr_P( test, S);

      u.F = p->F * (1-error); synthesize( S, 0, &u);
      forward_fft( S);
      r2 = corr_P( test, S);

      if( r2 < r1) r1 = r2;

      if( FLAG_V)
         fprintf( stderr, 
               "trying F error %.2f factor=%.2f targ=%.3f r1=%.3f\n",
                              100 * error, factor, target_cor, r1);

      if( r1 < target_cor)
      {
         error /= factor;
         factor = sqrt( factor);
      }

      error *= factor;
      if( error >= 1) 
      {
         error = 1.0;
         break;
      }
   }

   free( test); free( S);

   max_error = 1.0/(end_n - start_n + 1)/2.0;
   return error < max_error ? max_error : error;
}

static double estimate_Q_error( COMPLEX *T, struct PEAK *p)
{
   struct PEAK u = *p;
   COMPLEX *test = ca_alloc();
   COMPLEX *S = ca_alloc();

   double target_cor, r1, r2, error = 0.01/100;
   double factor = 2;

   synthesize( S, 0, p);
   target_cor = corr_M( S, T);

   while( factor > 1.1)
   {
      u.Q = p->Q * (1+error); synthesize( test, 0, &u);
      r1 = corr_M( test, S);
      if( isnan( r1))
         fprintf( stderr, "nan r1, error=%.3f\n", error);
      u.Q = p->Q * (1-error); synthesize( test, 0, &u);
      r2 = corr_M( test, S);
      if( isnan( r2))
         fprintf( stderr, "nan r2, error=%.3f\n", error);

      if( isnan( r1) || isnan( r2))
      {
         error = 1.0;
         break;
      }

      if( r2 < r1) r1 = r2;

      if( FLAG_V) 
         fprintf( stderr, 
                  "trying Q error %.2f factor %.2f targ=%.8f r1=%.8f\n",
                                  100 * error, factor, target_cor, r1);
      if( r1 < target_cor) 
      {
         error /= factor;
         factor = sqrt( factor);
      }
      error *= factor;
      if( error >= 1) 
      {
         error = 1.0;
         break;
      }
   }

   free( test); free( S);
   return error;
}

static void report_results( FILE *f)
{
   int peak;
   COMPLEX *S = ca_alloc();
   COMPLEX *test = ca_alloc();
   double y, mag;

   mag = rms_magnitude( inputdata);
   ca_zero( test);

   fprintf( f, "PK  FREQ kHz (Error +/-)    Q FACTOR (Error +/-)");
   fprintf( f, "   LEVEL\n");

   for( peak = from_peak; peak <= to_peak; peak++)
   {
      char temp[50];
      double m;
      struct PEAK *p = peaks + peak;

      if( !p->rcnt) continue;

      synthesize_noprecond( S, 1, p);
      ca_mul( S, p->G);
      ca_add( test, S);
      m = rms_magnitude( S);

      fprintf( f, "%2d %8.3f ", peak+1, p->F/1000);
      sprintf( temp, "(%.2f%%,%.0fHz)",
                       100 * p->error_F, p->error_F * p->F);
      fprintf( f, "%-15s ", temp);
      fprintf( f, "%7.2f ", p->Q);
      sprintf( temp, "(%.2f%%,%4.1f)",
                        100 * p->error_Q, p->error_Q * p->Q);
      fprintf( f, "%-13s ", temp);

      fprintf( f, "%.1fdB\n", 20 * log10(m/mag));
   }

   y = corr_X( test, inputdata);

   free( test);  free( S);

   fprintf( f, "Accounted for %.2f%% of input signal\n", y * 100);
}

static void output_TD2( void)
{
   int peak, i;
   COMPLEX *S = ca_alloc();
   COMPLEX *test = ca_alloc();

   ca_zero( test);

   for( peak = from_peak; peak <= to_peak; peak++)
   {
      struct PEAK *p = peaks + peak;

      if( !p->rcnt) continue;

      synthesize_noprecond( S, 1, p);
      ca_mul( S, p->G);
      ca_add( test, S);
   }

   for( i=1; i<=MAXDATA; i++)
      printf( "%.6e %.6e %.6e\n", (i-1)*DT, inputdata[i].re, test[i].re);

   free( test);  free( S);
}

void refine_single_peak( struct PEAK *p)
{
   COMPLEX *synthdata = ca_alloc();
   COMPLEX *tracedata = ca_alloc();
   struct PEAK t = *p;
   int n_min = 0;
   double error, min_error = -1;
   int i, n = 0;
      
   ca_copy( tracedata, inputdata);
   
   if( FLAG_DUAL)
      for( i=from_peak; i<= to_peak; i++)
         if( peaks + i != p && peaks[i].rcnt)
         {
            COMPLEX *S = ca_alloc();
            if( FLAG_V) fprintf( stderr, "subtracting peak %d\n", i+1);
            synthesize_noprecond( S, 1, peaks + i);
            ca_mul( S, peaks[i].G);
            ca_sub( tracedata, S);
            free( S);
         }
   
   precondition_peak( tracedata, p);
   // precondition( tracedata);
   forward_fft( tracedata);
   crop_ft( tracedata, F_LB, F_UB);
   reverse_fft( tracedata);
     
   heading_progress();

   do
   {  
      t.Q = find_Q( tracedata, &t);
      if( FLAG_V)
      {
         synthesize( synthdata, 0, &t);
         report_progress( tracedata, synthdata, 0, 'Q', &t);
      }

      t.F = find_F( tracedata, &t);
      if( FLAG_V) 
      {
         synthesize( synthdata, 0, &t);
         report_progress( tracedata, synthdata, 0, 'F', &t);
      }
      
      t.P = find_P( tracedata, &t);
      synthesize( synthdata, 0, &t);
      error = report_progress( tracedata, synthdata, 1, 'P', &t);
   
      #define ETRUNC(A) ((int)((A) * 1000))
      if( min_error < 0 || ETRUNC(error) < ETRUNC(min_error)) 
      {
         *p = t;
         min_error = error; 
         n_min = n;
      }
      else
      if( ETRUNC(error) >= ETRUNC(min_error) && n > n_min + 3) break;
   
      n++;
   }
    while( corr_X( synthdata, tracedata) < 0.998 || 
           corr_P( synthdata, tracedata) < 0.990);
   
   if( FLAG_V) report_progress( tracedata, synthdata, 0, 'H', p);
   
   p->error_F = estimate_F_error( tracedata, p);
   p->error_Q = estimate_Q_error( tracedata, p);
   
   ca_copy( tracedata, inputdata);
   forward_fft( tracedata);
   crop_ft( tracedata, F_LB, F_UB);
   reverse_fft( tracedata);
   synthesize_noprecond( synthdata, 0, p);
      
   p->G = compare_G( tracedata, synthdata);
   p->rcnt++;
   free( synthdata);  free( tracedata); 
}

static void output_functions( struct PEAK *p)
{ 
   COMPLEX *tracedata = ca_alloc();

   ca_copy( tracedata, inputdata);
   precondition_peak( tracedata, p);
   // precondition( tracedata);
   forward_fft( tracedata);
   crop_ft( tracedata, F_LB, F_UB);
   reverse_fft( tracedata);
   
   if( FLAG_FCMP)
   {
      output_F_function( p);
      exit( 0);
   }
   if( FLAG_TD1)
   {
      output_TD1( tracedata, p);
      exit( 0);
   }
   if( FLAG_TD0)
   {
      output_TD0( inputdata, p);
      exit( 0);
   }
   if( FLAG_PCMP)
   {
      output_P_function( tracedata, p);
      exit( 0);
   }
   if( FLAG_QCMP)
   {
      output_Q_function( tracedata, p);
      exit( 0);
   }

   free( tracedata);
}

/*
 *  Sort out the command line options.
 */

static void usage( void)
{
   fprintf( stderr, "tcma version %s\n", VERSION);
   fprintf( stderr, "usage: ./tcma [options] < input_file\n");
   fprintf( stderr, " options are:\n");
   fprintf( stderr, "   -ft          Show fourier transform of input\n");
   fprintf( stderr, "   -signals     List approximate signal components\n");
   fprintf( stderr, "   -peak <n>    Process only peak number n\n");
   fprintf( stderr, "   -npeaks <n>  Process only the strongest n signals\n");
   fprintf( stderr, "   -dual        Optimise for separating close peaks\n");
   fprintf( stderr, "   -t <step>    Time step in uS for single column data\n");
   fprintf( stderr, "   -s8          Input file is in signed bytes\n");
   fprintf( stderr, "   -u8          Input file is in unsigned bytes\n");
   fprintf( stderr, "   -v           Verbose mode\n");
   fprintf( stderr, "   -q           Run quietly\n");
   exit( 1);
}

void decode_options( int argc, char *argv[])
{
   int i;

   for( i=1; i<argc; i++)
   {
      if( !strcmp( argv[i], "-ft")) FLAG_FT = 1;
      else
      if( !strcmp( argv[i], "-td1")) FLAG_TD1 = 1;
      else
      if( !strcmp( argv[i], "-td2")) FLAG_TD2 = 1;
      else
      if( !strcmp( argv[i], "-td0")) FLAG_TD0 = 1;
      else
      if( !strcmp( argv[i], "-fcmp")) FLAG_FCMP = 1;
      else
      if( !strcmp( argv[i], "-qcmp")) FLAG_QCMP = 1;
      else
      if( !strcmp( argv[i], "-pcmp")) FLAG_PCMP = 1;
      else
      if( !strcmp( argv[i], "-dual")) FLAG_DUAL = 1;
      else
      if( !strcmp( argv[i], "-q")) FLAG_Q = 1;
      else
      if( !strcmp( argv[i], "-u8")) FLAG_U8 = 1;
      else
      if( !strcmp( argv[i], "-s8")) FLAG_S8 = 1;
      else
      if( !strcmp( argv[i], "-t") && ++i < argc)
         FLAG_TSTEP = atof( argv[i]);
      else 
      if( !strcmp( argv[i], "-signals")) FLAG_SIGNALS = 1;
      else
      if( !strcmp( argv[i], "-npeaks") && ++i < argc) 
         FLAG_PMAX = atoi( argv[i]);
      else
      if( !strcmp( argv[i], "-v")) FLAG_V = 1;
      else
      if( !strcmp( argv[i], "-pre")) FLAG_PRE = 1;
      else
      if( !strcmp( argv[i], "-peak") && ++i < argc) do_peak = atoi( argv[i])-1;
      else
      if( !strcmp( argv[i], "-fmin") && ++i < argc) 
         fmin = atof( argv[i]) * 1000;
      else
      if( !strcmp( argv[i], "-fmax") && ++i < argc) 
         fmax = atof( argv[i]) * 1000;
      else
      if( !strcmp( argv[i], "-ustart") && ++i < argc) 
         FLAG_start_us = atof( argv[i]);
      else
      if( !strcmp( argv[i], "-uend") && ++i < argc) 
         FLAG_end_us = atof( argv[i]);
      else
         usage();
   }
}

int main( int argc, char *argv[])
{
   int i, r;
   int peak;

   decode_options( argc, argv);

   /*
    *  Read in the raw waveform data.
    */

   read_raw_data();
   crop_td();

   /*
    *  Fourier transform and find the signal peaks.
    */

   scan_peaks();

   if( FLAG_SIGNALS)
   {
      for( i=0; i<npeaks; i++)
         printf( "peak %d: %.0f Hz %.3f\n", i+1, peaks[i].F, peaks[i].v);
      exit( 0);
   }

   if( FLAG_FT)
   {
      output_FT();
      exit( 0);
   }

   if( do_peak >= 0)
   {
      if( do_peak >= npeaks) bailout( "no peak %d", do_peak+1);
      from_peak = to_peak = do_peak;
   }
   else
   {
      from_peak = 0;
      to_peak = npeaks - 1;
   }

   for( r = 0; r< 3; r++)
   {
      int d1, d2, flag = 0;

      if( r && !FLAG_DUAL) break;

      if( FLAG_DUAL) for( d1 = from_peak; d1 <= to_peak; d1++)
         for( d2 = d1+1; d2 <= to_peak; d2++)
         {
            double r = peaks[d1].F/peaks[d2].F;
            if( r < 1) r = 1/r;
            if( r < 1 + 8.0*FILTER_BW/100.0) flag++;
         }

      if( r && !flag) break;

      for( peak = from_peak; peak <= to_peak; peak++)
      {
         struct PEAK *p = peaks + peak;

         p->Qx = p->Q;
  
         if( !FLAG_Q) 
            fprintf( stderr, "%s work on peak %d: %.3f kHz Q=%.3f\n", 
                p->rcnt ? "Continuing" : "Starting", peak+1, p->F/1000, p->Q);
   
         /*
          *  Setup filter bounds for this peak.
          */
     
         F_LB = p->F*(1 - FILTER_BW/100.0);
         F_UB = p->F/(1 - FILTER_BW/100.0);
      
         if( FLAG_V)
            fprintf( stderr, 
               "using %.1f cycles, %d steps, %.0f%% of trace\n", 
                    (end_n - start_n + 1) * DT * p->F,
                    (end_n - start_n + 1),
                    100 * (end_n - start_n + 1)/(double)MAXDATA);
   
      
         if( FLAG_V) 
            fprintf( stderr, "steps per cycle: %d\n", (int)(1/p->F/DT));
         if( !FLAG_Q && 1/p->F/DT < 3.5)
            fprintf( stderr, "this could be tricky\n");
   
         if( FLAG_V) fprintf( stderr, "max possible resolution: %.1f Hz\n", 
                          p->F/(end_n - start_n + 1) );
   
         if( FLAG_PRE) output_functions( p); 
    
         refine_single_peak( p);
  
         if( !FLAG_Q)
         { 
            if( !FLAG_V) fprintf( stderr, "\n");
            report_results( stderr);
         }

         output_functions( p);
      }
   }

   if( FLAG_Q) report_results( stdout);
   if( FLAG_TD2) output_TD2();
   return 0;
}