/**********************************************
* Filename : Goertzel.CPP                     *
* Function : Detect whether or not there is   *
             the special frequency signal     *
*Parameter : (le) the length of input data    *
             (x) the input data               *
			 (ls) the length of the section   *
			      detected                    *
			 (m) le=m*ls                      *
			 (fs) the sample frequency        *
			 (f) the detected frequency       *
			 (result) the result of detection * 
**********************************************/


#include "math.h"

void Goertzel(int le,float *x,int ls,int m,float fs,float f,float *result)
{
	int k,i,j;
	float w,temp,v[3],Valve;
    Valve=600.0;    /* Valve is related to the amplitude of x, here x=[-1,1] */
	
/* calculate w (the circular frequency of f) */
	temp=f*(float)ls/fs;
	k=(int)temp;
	temp=temp-(float)k;
	if(temp>0.50)
	{
		k++;
	}
	w=2*cos(8.0*atan(1.0)*(double)k/ls);

/* calculate result recursively */
	for(i=0;i<m;i++)
	{
		v[0]=0;v[1]=0;v[2]=0;
		for(j=0;j<ls;j++)
		{
			v[2]=w*v[1]-v[0]+(*(x+(i*ls+j)));
			v[0]=v[1];
			v[1]=v[2];
		}
		temp=v[1]*v[1]+v[0]*v[0]-w*v[0]*v[1]-Valve;
		if(temp<0)
		{
			temp=0.0;
		}
		*(result+i)=temp;
	}
}