import java.lang.Math;
import java.lang.Byte; 

/** 
 * The Goertzel class can be used to perform the Goertzel algorithm.
 * This class is based on a C program implemented by Kevin Banks of 
 * Embedded Systems Programming.
 *
 * @author  Chris Palistrant, Tony Offer
 * @version 0.0, May 2004
 */
public class Goertzel{

    float SAMPLING_RATE;
    float TARGET_FREQUENCY;
    int N;
    double[] testData;

    double coeff, Q1, Q2;
    double sine, cosine;
    
    boolean DEBUG = false;
    
    /**
     * Default Constructor.  This constructor assumes that the smapling rate
     * 44.1kHz, and the frequency to search for is 21kHz
     * 
     */
    Goertzel(){
	this(44100.0F, 21000.0F, 420);
	//this(8000.0F,941.0F,205);
    }
	
    /**
     * Constructor
     * @param sampleRate is the sampling rate of the signal to be analyzed
     * @param targetFreq is the frequency that Goertzel will look for.
     * @param n is the block size to use with Goertzel
     */
    Goertzel(float sampleRate, float targetFreq, int n){
	SAMPLING_RATE = sampleRate;
        TARGET_FREQUENCY = targetFreq;
	N = n;
	testData = new double[N];
    }

    /**
     * Call this method for every sample.
     *  
     * @return void
     */
    void resetGoertzel()
    {
       Q2 = 0;
       Q1 = 0;
    }
    
    /**
     * Call this once, to precompute the constants.
     *  
     * @return void
     */
    void initGoertzel()
    {
	int	k;
	float	floatN;
	double	omega;

	floatN = (float) N;
	k = (int) (0.5 + ((floatN * TARGET_FREQUENCY) / SAMPLING_RATE));
	omega = (2.0 * Math.PI * k) / floatN;
	sine = Math.sin(omega);
	cosine = Math.cos(omega);
	coeff = 2.0 * cosine;
	
	System.out.println("For SAMPLING_RATE = " + SAMPLING_RATE);
	System.out.print(" N = " + N);
	System.out.println(" and FREQUENCY = " + TARGET_FREQUENCY);
	System.out.println("k = " + k + " and coeff = " + coeff + "\n");
	
	resetGoertzel();
    }

    /**
     * Call this routine for every sample.
     *  
     * @param sample is a double
     * @return void
     */
    void processSample(double sample)
    {
        double Q0;
	Q0 = coeff * Q1 - Q2 + sample;
	Q2 = Q1;
	Q1 = Q0;
//	if(DEBUG){
//	    System.out.println("processSample.Q0 " + Q0 +
//			       "processSample.Q2 " + Q2 +
//			       "processSampleQ1 " + Q1);
//        }
    }

    /**
     * Basic Goertzel.  Call this routine after every block to get the
     * complex result.
     *  
     * @param parts has length two where the first item is the real 
     *     part and the second item is the complex part.
     * @return double[] stores the values in the param
     */
    double[] getRealImag(double[] parts)
    {
	parts[0] = (Q1 - Q2 * cosine);
        parts[1] = (Q2 * sine);
	return parts;
    }

    /**
     * Optimized Goertzel.Call this after every block to get the 
     * RELATIVE magnitude squared.
     *  
     * @param 
     * @return double is the value of the relative mag squared.
     */
    double getMagnitudeSquared()
    {
	return (Q1 * Q1 + Q2 * Q2 - Q1 * Q2 * coeff);
    }

    /**
     * End of Goertzel-specific code, the remainder is test code. 
     */

    /** 
     * Pass in some test data. 
     *
     */
    void signal(double[] frequency)
    {
	int x = N;
	if(frequency.length < N){
	    for (int i = frequency.length; i < x; i++)
		testData[i] = 0;
	    x = frequency.length;
	}    
	    
	for (int index = 0; index < x; index++)
	{
	    testData[index] = frequency[index];
	}
    }

    /** 
     * Synthesize some test data at a given frequency. 
     */
    void generate(double frequency)
    {
	double step;

	step = (frequency * ((2.0 * Math.PI) / SAMPLING_RATE));
	
	/* Generate the test data */
	for (int index = 0; index < N; index++)
	{
	    testData[index] = (100.0 * Math.sin(index * step) + 100.0);
	    if (DEBUG){
		System.out.println("generate.index " + index);
		System.out.println("generate.testData " + (int)testData[index]);
		System.out.println("generate.Step: " + step);
		System.out.println("generate.Sine " +Math.sin(index * step));
	    }
	}
    }

/* Demo 1 */
    void generateAndTest(double frequency)
    {
	int	index;

	double	magnitudeSquared;
	double	magnitude;
	double[] parts = new double[2]; 
	double  real;
	double	imag;

	System.out.println("For test frequency " +  frequency);
	generate(frequency);

	/* Process the samples */
	for (index = 0; index < N; index++)
	{
	    processSample(testData[index]);
	}

	/* Do the "basic Goertzel" processing. */
	parts = getRealImag(parts);
	real = parts[0];
	imag = parts[1];

	System.out.println("real = " + real + " imag = " + imag);
	
	magnitudeSquared = real*real + imag*imag;
	System.out.println("Relative magnitude squared = " + magnitudeSquared);
	magnitude = Math.sqrt(magnitudeSquared);
	System.out.println("Relative magnitude = " + magnitude);

	/* Do the "optimized Goertzel" processing */
/*	magnitudeSquared = GetMagnitudeSquared();
	printf("Relative magnitude squared = %f\n", magnitudeSquared);
	magnitude = sqrt(magnitudeSquared);
	printf("Relative magnitude = %f\n\n", magnitude);*/
	
	resetGoertzel();
}

    //Demo2
    void GenerateAndTest2(double frequency){
    
        int	index;

	double	magnitudeSquared;
	double	magnitude;
	double	real;
	double	imag;
	double[] parts = new double[2];

	System.out.println("Freq=  " + frequency);
	generate(frequency);

	/* Process the samples. */
	for (index = 0; index < N; index++)
	{
	    processSample(testData[index]);
	}

	/* Do the "standard Goertzel" processing. */
	parts = getRealImag(parts);
	real = parts[0];
	imag = parts[1];

	magnitudeSquared = real*real + imag*imag;
	System.out.println("rel mag^2= " + magnitudeSquared);
	magnitude = java.lang.Math.sqrt(magnitudeSquared);
	System.out.println("rel mag= " + magnitude);

	resetGoertzel();
    }


    /**
     * The main function used for testing.
     *  
     * @param
     * @return 
     */
    public static void main(String[] args){

	Goertzel test = new Goertzel();
	if(args.length > 0 && args[0].equals("-d"))
	   test.DEBUG = true;
	test.initGoertzel();

	//Demo 1
        test.generateAndTest(250);
	test.generateAndTest(test.TARGET_FREQUENCY);

    }
}