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


/* Standard functions from "Numerical Recipies" book */
/*****************************************************/

#define NR_END    1
#define FREE_ARG  char *


static float sqrarg;
#define SQR(a)  ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
/*************/



#define MAX_HARM 200

#define PI2   6.28318530717959


#define SWAP(a,b)   tempr=(a); (a)=(b); (b)=tempr

#define WINDOW(j,a,b) (1.0-fabs((((j)-1)-(a))*(b)))   /* Bartlett */






float sumw2048;   /* calc on init */
float window[2048*2 + 1];




int  data_in[1024+1];
int  data[512+1];
float result[2049];
float harmf[MAX_HARM];

int max_data = 0;
int min_data = 0;






void ipolint(int xa[], int ya[], int n, int x, int *y)
/****************************************************/
/* Polynomial Interpolation (integer version), n must be <= 4 */
{
   int  i, m, ns=1;
   int den, dif, dift, ho, hp, w;
   int  result;
   int c[6], d[6];

   dif = abs(x-xa[1]);

   for(i=1; i<=n; i++)
   {
      if( (dift = abs(x-xa[i])) < dif)
      {
         ns = i;
         dif = dift;
      }
      c[i] = ya[i] << 6;
      d[i] = ya[i] << 6;
   }
   result = (ya[ns--]) << 6;

   for(m=1; m<n; m++)
   {
      for(i=1; i<=n-m; i++)
      {
         ho = xa[i]-x;
         hp = xa[i+m] - x;
         w = c[i+1] - d[i];
         den = ho-hp;
         d[i] = (hp*w)/den;
         c[i] = (ho*w)/den;
      }
      result += ((2*ns < (n-m) ? c[ns+1] : d[ns--]));
   }
   *y = result >> 6;
}   /* end of ipolint */




void nrerror(char error_text[])
/****************************/
/* Numerical recipies standard error handler */
{
	fprintf(stderr,error_text);
}


float *vector(long n1, long nh)
/*****************************/
/* Allocate a float vector with subscript range v[n1..nh] */
{
	float *v;
	size_t size;

	size = (nh-n1+1+NR_END)*sizeof(float);

	v = (float *)malloc(size);
	if(!v)  nrerror("allocation failure in vector()");
	return(v-n1+NR_END);
}


int *ivector(long n1, long nh)
/****************************/
/* Allocate an int vector with subscript range v[n1..nh] */
{
	int *v;

	v = (int *)malloc((size_t)((nh-n1+1+NR_END)*sizeof(int)));
	if(!v)  nrerror("allocation failure in ivector()");
	return(v-n1+NR_END);
}


void free_vector(float *v, long n1, long nh)
/******************************************/
{
	free((FREE_ARG)(v+n1-NR_END));
}


void free_ivector(int *v, long n1, long nh)
/*****************************************/
{
	free((FREE_ARG)(v+n1-NR_END));
}






void four1(float data[], unsigned long nn, int isign)
{//==================================================
	unsigned long n, mmax, m , j, istep, i;
	double wtemp, wr, wpr, wpi, wi, theta;
	float tempr, tempi;

	n = nn << 1;
	j = 1;

	for(i=1; i<n; i+=2)
	{
		if(j > i)
		{
			SWAP(data[j],data[i]);
			SWAP(data[j+1],data[i+1]);
		}
		m = n >> 1;
		while(m >= 2 && j > m)
		{
			j -= m;
			m >>= 1;
		}
		j += m;
	}

	// Here beings the Danielson-Lanczos section of the routine
	mmax = 2;
	while(n > mmax)
	{
		istep = mmax << 1;
		theta = isign * (PI2/mmax);
		wtemp = sin(0.5 * theta);
		wpr = -2.0 * wtemp * wtemp;
		wpi = sin(theta);
		wr = 1.0;
		wi = 0.0;

		for(m=1; m<mmax; m+=2)
		{
			for(i=m; i<=n; i+=istep)
			{
				j = i + mmax;
				tempr = wr * data[j] - wi * data[j+1];
				tempi = wr * data[j+1] + wi * data[j];
				data[j] = data[i] - tempr;
				data[j+1] = data[i+1] - tempi;
				data[i] += tempr;
				data[i+1] += tempi;
			}
			wr = (wtemp=wr)*wpr - wi*wpi + wr;
			wi = wi*wpr + wtemp*wpi + wi;
		}
		mmax = istep;
	}
}



#ifdef deleted
void four2(float datain[], unsigned long nn, int isign)
/***************************************************/
/* Replaced data[1..2*nn] by its discrete Fourier transform......

	nn MUST be an integral power of 2 (this is not checked for)

nn = 2048*2
eg: data[2048*4], nn=2048*2
*/
{
	unsigned long n, mmax, m, j, istep, i;

	/* double precision for the trig. recurrences */
	double wtemp, wr, wpr, wpi, wi, theta;

	int iwi, iwr, tempi, tempr;
	int  x1, x2;

	int data[2048*4 +1];


	for(i=1; i<=(2048*4); i++)
	{
		data[i] = (int)(datain[i] * 64);
	}

	n = nn << 1;
	j = 1;
	for(i=1; i<n; i+=2)
	{
		if(j > i)
		{
			SWAP(data[j],data[i]);
			SWAP(data[j+1], data[i+1]);
		}
		m = n >> 1;

		while((m >= 2) && (j > m))
		{
			j -= m;
			m >>= 1;
		}
		j += m;
	}

	/* here begins the danielson-lanczos section of the routine */

	mmax = 2;
	while(n > mmax)      /* outer loop executed log2 nn  times */
	{
		istep = mmax << 1;
		theta = isign * (PI2 / mmax);
		wtemp = sin(0.5 * theta);
		wpr = -2.0 * wtemp * wtemp;

		wpi = sin(theta);
		wr = 1.0;
		wi = 0.0;


/* NOTE:
	With 16 bit data[], tempi, tempr range +/- 10E+6 (10,000,000 or 2^24)
	all w** +/- 1.0 except wpr  0 to -2.0
*/

		for(m=1; m<mmax; m+=2)
		{
			iwi = (int)(wi * 0x40000000);
			iwr = (int)(wr * 0x40000000);

			for(i=m; i<=n; i+=istep)
			{
				j = i+mmax;         /* danielson-lanczos formula */

				tempr = mul(iwr,data[j]) - mul(iwi,data[j+1]);
				tempi = mul(iwr,data[j+1]) + mul(iwi,data[j]);

				data[j] = data[i] - tempr;
				data[j+1] = data[i+1] - tempi;
				data[i] += tempr;
				data[i+1] += tempi;
			}
			wr = (wtemp=wr)*wpr-wi*wpi+wr;    /* trig recurrence */
			wi=wi*wpr+wtemp*wpi+wi;
		}
		mmax = istep;
	}

	for(i=1; i<=(2048*4); i++)
	{
if(data[i] > max_data) max_data = data[i];
if(data[i] < min_data) min_data = data[i];
		datain[i] = (float)data[i] / 64.0;
	}
}   /* end four2 */
#endif



void spctrm2(int *data, int *data_end,
									float p[], int m, int k, int overlap)
/**************************************************************************/
{
	void four1(float data[], unsigned long nn, int isign);

	int *dp;
	int mm, m44, m43, m4, kk, joffn, joff, j2, j;
	float w, facp, facm, *w1, *w2, sumw=0.0, den=0.0;

	int integer_fourier = 0;

	/* useful factors */
	mm = m+m;
	m43 = (m4=mm+mm)+3;
	m44 = m43+1;

	w1 = vector(1,m4);
	w2 = vector(1,m);

	facm = m;
	facp = 1.0/m;

	dp = data;

	/* (calculate 'sumw'  This has been moved into init_fourier) */

	/* accumulate the squared sum of the weights */
	for(j=1; j<=m; j++)
		p[j] = 0.0;        /* initialise spectrum to zero */

	if(overlap)
	{
		/* initialise the "save" half buffer */
		for(j=1; j<=m; j++)
		{
#ifdef deleted
			if(dp >= data_end)
				w2[j] = 0.0;
			else
				w2[j] = (float)(*dp++);  /* TRY filling excess with zeros */
#endif
			w2[j] = (float)(*dp++);
			if(dp == data_end)  dp=data;   /* repeat same data again */

		}
	}

	for(kk=1; kk<=k; kk++)
	{
		/* loop over data set segments in groups of two */
		for(joff= -1; joff<=0; joff++)
		{
			/* get two complete segments of workspace */
			if(overlap)
			{
				for(j=1; j<=m; j++)   w1[joff+j+j] = w2[j];
				for(j=1; j<=m; j++)
				{
					w2[j]=(float)(*dp++);
					if(dp == data_end)   dp = data;
				}

				joffn = joff + mm;
				for(j=1; j<=m; j++)   w1[joffn+j+j] = w2[j];
			}
			else
			{
				for(j=joff+2; j<=m4; j+=2)
				{
					w1[j] = (float)(*dp++);
					if(dp == data_end)   dp = data;
				}
			}
		}

		/* apply the window to the data */
		for(j=1; j<=mm; j++)
		{
			j2 = j+j;
			w = window[j];    /* window is pre-calculated in init_fourier */

			w1[j2] *= w;
			w1[j2-1] *= w;
		}


		four1(w1,mm,1);      /* fourier transform the windowed data */


		p[1] += (SQR(w1[1]) + SQR(w1[2]));   /* sum results into prev. segments */

		for(j=2; j<=m; j++)
		{
			j2 = j+j;
			p[j] += (SQR(w1[j2]) + SQR(w1[j2-1])
						+ SQR(w1[m44-j2]) + SQR(w1[m43-j2]));
		}
		den += sumw2048;
	}

	den *= m4;        /* correct normalisation */
	for(j=1; j<=m; j++)   p[j] /= den;


	free_vector(w2,1,m);
	free_vector(w1,1,m4);

}   /* end of spctrm2 */






void init_fourier()
/*****************/
{
	int  i;
	int  j;
	float facm, facp, w;


	/* just do this once for standard size of 2048 output bins */
	facm = 2048;
	facp = 1/2048.0;
	sumw2048 = 0.0;

	for(j=1; j<=(2048*2); j++)
	{
		w = WINDOW(j, facm, facp);
		window[j] = w;
		sumw2048 += SQR(w);
	}

}   /* end of init_fourier */




void stretch_wave(int *w, int length, int *data, int nlen_pow)
/**************************************************************/
/* Stretch wave 'w' of length 'length' bytes to 512 bytes in 'data'
nlen_pow = 9 for 512, 10 for 1024 */
{
	int  i;
	int  l;
	int  x;
	int  dl;
	int  xa1;
	int  y;
	int  nlen;
	int  xa[5];
	int  ya[5];

	nlen = 1 << nlen_pow;

	data[0] = w[0]*2;

	dl = nlen * 256;
	dl = dl / length;

	for(x=1; x<nlen; x++)
	{
		l = x * length;
		l = l >> nlen_pow;
		if(l >= length) l = length-1;

		ya[1] = w[l-1]*2;
		ya[2] = w[l]*2;

		if(l < (length - 1))
			ya[3] = w[l+1]*2;
		else
			ya[3] = w[l]*2;

		if(l < (length - 2))
			ya[4] = w[l+2]*2;
		else
			ya[4] = w[l]*2;

		xa[1] = (l-1) *dl;
		xa[2] = xa[1] + dl;
		xa[3] = xa[2] + dl;
		xa[4] = xa[3] + dl;

		ipolint(xa,ya,4,x*256,&y);
		data[x] = y;
	}
}   /* end of stretch_wave */




void analyse_cycle(short *w, int length, float *harmf, int options)
/*****************************************************************/
/* Perform fourier analysis on one cycle of a sampled wave/
	w = input wave
	length = number of samples

	harm       result, relative strengths of harmonics
	options    bit 0  1=take sqrt
				  bit 1  1=keep relative amplitude of each cycle
*/
{
	int  i;
	int  x;
	int  h;
	signed char *bytes;
	float max;
	float bin;


	/* trunkate wave if too long */
	if(length > 1024)
		length = 1024;

	/* copy into data buffer */
	for(i=0; i<length; i++)
	{
		data_in[i] = *w++;
	}


	/* stretch wave by interpolation to 512 samples */
	stretch_wave(data_in,length,data,9);

	/* perform fourier analysis, 4 bins per harmonic */
init_fourier();
	spctrm2(data,&data[512],result,2048,1,0);


	/* get bin plus adjacent bins for each harmonic */
	max = 0;
	for(h=1; h<MAX_HARM; h++)
	{
		bin = result[h*8] + result[h*8+1] + result[h*8+2];
		harmf[h] = sqrt(bin);   /* convert from power to amplitude */

		if(options & 1)
		{
			harmf[h] = sqrt(harmf[h]);    /* take square route of amplitude */
		}

		if(harmf[h] > max)
			max = harmf[h];
	}
}   /* end of analyse_cycle */



