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.  The smapling rate is set to 
     * 44.1kHz, and the frequency to search for is 21kHz.  An appropriate
     * bin value is also chosen.
     * 
     */
    public Goertzel(){
	this(44100.0F, 21000.0F, 420);
	//this(8000.0F,941.0F,205);
    }
	
    /**
     * A Constructor that takes parameters.
     *
     * @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
     */
    public Goertzel(float sampleRate, float targetFreq, int n){
	SAMPLING_RATE = sampleRate;
        TARGET_FREQUENCY = targetFreq;
	N = n;
	testData = new double[N];
    }

    /**
     * Call this method after every bin of size N is analyzed.
     *  
     * @return void
     */
    public void resetGoertzel()
    {
       Q2 = 0;
       Q1 = 0;
    }
    
    /**
     * Call this once, to precompute the constants.
     *  
     * @return void
     */
    public 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;
	if(DEBUG){	
	    System.out.println("Goertzel: For SAMPLING_RATE = " + SAMPLING_RATE);
	    System.out.print("Goertzel:  N = " + N);
	    System.out.println("Goertzel: and FREQUENCY = " + TARGET_FREQUENCY);
	    System.out.println("Goertzel: k = " + k + " and coeff = " + coeff + "\n");
	}
	
	resetGoertzel();
    }

    /**
     * Call this routine for every sample.
     *  
     * @param sample is a double.
     * @return void
     */
    public void processSample(double sample)
    {
        double Q0;
	Q0 = coeff * Q1 - Q2 + sample;
	Q2 = Q1;
	Q1 = Q0;
    }

    /**
     * Basic Goertzel.  Call this routine after every block to get the
     * complex result.
     *  
     * @param parts has length of 2 where the first item is the real 
     *     part and the second item is the complex part.
     * @return double[] stores the values in the respective param parts
     */
    public 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.
     *  
     * @return double is the value of the relative mag squared.
     */
    public double getMagnitudeSquared()
    {
	return (Q1 * Q1 + Q2 * Q2 - Q1 * Q2 * coeff);
    }

    
//---------End of Goertzel-specific code, the remainder is test code. ----//
    
    /** 
     * Synthesize some test data at a given frequency. 
     */
    private 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("Goerztel.generate.index " + index);
		System.out.println("G...generate.testData " + (int)testData[index]);
		System.out.println("G...generate.Step: " + step);
		System.out.println("G...generate.Sine " +Math.sin(index * step));
	    }
	}
    }

    /**
     * Demo 1: Generates and test a sample of a given frequency
     */
    private 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();
	System.out.println("Relative magnitude squared = " + magnitudeSquared);
	magnitude = Math.sqrt(magnitudeSquared);
	System.out.println("Relative magnitude = " + magnitude);
	
	resetGoertzel();
    }

    /**
     * Demo2: Should be called in a for loop to results of various freqs.
     */
    private 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.
     * The option "-d" will allow a text output of the 
     * current status of the program.
     *  
     * @param args can be the string "-d" for debug mode.
     */
    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);
	System.out.println("");
	test.generateAndTest(test.TARGET_FREQUENCY);
    }
}